├── README.md ├── bin ├── BOOT.BIN ├── StereoBM.elf ├── boot.scr ├── design_1_wrapper.bit ├── design_1_wrapper.xsa └── image.ub ├── data ├── ref_rect_l.zip ├── ref_rect_r.zip ├── ref_xsbl_l.zip └── ref_xsbl_r.zip ├── doc ├── chessboard.pdf ├── fpga │ └── RegisterMap.xlsx └── petalinux │ ├── linux.bif │ ├── myinit │ ├── myinit.bb │ ├── pl.dtsi │ ├── run │ └── system-user.dtsi ├── src ├── StereoBM │ └── src │ │ ├── Parameters.h │ │ ├── csi.c │ │ ├── csi.h │ │ ├── fpga.c │ │ ├── fpga.h │ │ ├── i2c.c │ │ ├── i2c.h │ │ ├── lscript.ld │ │ ├── lsm9ds1.c │ │ ├── lsm9ds1.h │ │ ├── main.c │ │ ├── main.h │ │ ├── ov5640.c │ │ ├── ov5640.h │ │ ├── uart.c │ │ ├── uart.h │ │ ├── xusb_ch9.c │ │ ├── xusb_ch9.h │ │ ├── xusb_ch9_video.c │ │ ├── xusb_ch9_video.h │ │ ├── xusb_main.c │ │ ├── xusb_main.h │ │ ├── xusb_wrapper.c │ │ └── xusb_wrapper.h ├── capture_video │ └── main.cpp ├── constr_1.xdc ├── dvp │ ├── ip │ │ ├── bm_calc_dly │ │ │ └── bm_calc_dly.xci │ │ ├── bm_obuf2_fifo │ │ │ └── bm_obuf2_fifo.xci │ │ ├── bm_obuf_fifo │ │ │ └── bm_obuf_fifo.xci │ │ ├── cmd_cue │ │ │ └── cmd_cue.xci │ │ ├── csi_dly_fifo │ │ │ └── csi_dly_fifo.xci │ │ ├── dpram_32_512 │ │ │ └── dpram_32_512.xci │ │ ├── dpram_36_512 │ │ │ └── dpram_36_512.xci │ │ ├── dpram_64_1024 │ │ │ └── dpram_64_1024.xci │ │ ├── fifo_frm │ │ │ └── fifo_frm.xci │ │ ├── gftt_box_ram │ │ │ └── gftt_box_ram.xci │ │ ├── gftt_ibuf_ram │ │ │ └── gftt_ibuf_ram.xci │ │ ├── gftt_obuf_fifo │ │ │ └── gftt_obuf_fifo.xci │ │ ├── gftt_sqrt │ │ │ └── gftt_sqrt.xci │ │ ├── grb_fifo │ │ │ └── grb_fifo.xci │ │ ├── rect_dly2 │ │ │ └── rect_dly2.xci │ │ ├── rect_line_buf │ │ │ └── rect_line_buf.xci │ │ ├── rect_obuf_dff │ │ │ └── rect_obuf_dff.xci │ │ └── tdpram_340_1024 │ │ │ └── tdpram_340_1024.xci │ ├── rtl │ │ ├── arb.v │ │ ├── axi_if.v │ │ ├── bm.v │ │ ├── bm_calc.v │ │ ├── bm_calc_det.v │ │ ├── bm_calc_frac.v │ │ ├── bm_calc_sad.v │ │ ├── bm_calc_uni.v │ │ ├── bm_calc_upd.v │ │ ├── bm_ibuf.v │ │ ├── bm_ibuf_lr.v │ │ ├── bm_ibuf_sad.v │ │ ├── bm_obuf.v │ │ ├── bm_obuf2.v │ │ ├── clkx.v │ │ ├── clkx2.v │ │ ├── csi_dly.v │ │ ├── csi_if.v │ │ ├── csi_if_ch.v │ │ ├── diven.v │ │ ├── dvp.v │ │ ├── frame_meas.v │ │ ├── gftt.v │ │ ├── gftt_box.v │ │ ├── gftt_eig.v │ │ ├── gftt_ibuf.v │ │ ├── gftt_obuf.v │ │ ├── gftt_sbl.v │ │ ├── grb.v │ │ ├── rect.v │ │ ├── rect_ibuf.v │ │ ├── rect_intp.v │ │ ├── rect_intp_fifo.v │ │ ├── rect_obuf.v │ │ ├── rect_rmp.v │ │ ├── simple_fifo.v │ │ └── xsbl2.v │ └── sim │ │ ├── cmd.dat │ │ ├── ddr_mdl.v │ │ ├── dummy_bm.v │ │ ├── dummy_gftt.v │ │ ├── dummy_rect.v │ │ ├── dummy_xsbl2.v │ │ ├── img_001_l.dat │ │ ├── img_001_l.png │ │ ├── img_001_r.dat │ │ ├── img_001_r.png │ │ ├── imread_csi.v │ │ ├── imread_dvp.v │ │ ├── sim_bm_obuf2.v │ │ ├── sim_bm_obuf2_behav.wcfg │ │ ├── sim_csi_if.v │ │ ├── sim_csi_if_behav.wcfg │ │ ├── sim_dvp.v │ │ ├── sim_dvp_bm.wcfg │ │ ├── sim_dvp_bm_all.wcfg │ │ ├── sim_dvp_csi.wcfg │ │ ├── sim_dvp_ffm.wcfg │ │ ├── sim_dvp_gftt.wcfg │ │ ├── sim_dvp_grb.wcfg │ │ ├── sim_dvp_i2c.wcfg │ │ ├── sim_dvp_rect.wcfg │ │ ├── sim_dvp_xsbl.wcfg │ │ ├── sim_gftt.v │ │ ├── sim_gftt.wcfg │ │ ├── sim_gpio.v │ │ ├── sim_gpio.wcfg │ │ ├── sim_sqrt.v │ │ └── sim_sqrt.wcfg ├── ip_repo │ └── memo.txt ├── slam │ ├── include │ │ ├── Eigen │ │ │ ├── Cholesky │ │ │ ├── CholmodSupport │ │ │ ├── Core │ │ │ ├── Dense │ │ │ ├── Eigen │ │ │ ├── Eigenvalues │ │ │ ├── Geometry │ │ │ ├── Householder │ │ │ ├── IterativeLinearSolvers │ │ │ ├── Jacobi │ │ │ ├── KLUSupport │ │ │ ├── LU │ │ │ ├── MetisSupport │ │ │ ├── OrderingMethods │ │ │ ├── PaStiXSupport │ │ │ ├── PardisoSupport │ │ │ ├── QR │ │ │ ├── QtAlignedMalloc │ │ │ ├── SPQRSupport │ │ │ ├── SVD │ │ │ ├── Sparse │ │ │ ├── SparseCholesky │ │ │ ├── SparseCore │ │ │ ├── SparseLU │ │ │ ├── SparseQR │ │ │ ├── StdDeque │ │ │ ├── StdList │ │ │ ├── StdVector │ │ │ ├── SuperLUSupport │ │ │ ├── UmfPackSupport │ │ │ └── src │ │ │ │ ├── Cholesky │ │ │ │ ├── LDLT.h │ │ │ │ ├── LLT.h │ │ │ │ └── LLT_LAPACKE.h │ │ │ │ ├── CholmodSupport │ │ │ │ └── CholmodSupport.h │ │ │ │ ├── Core │ │ │ │ ├── ArithmeticSequence.h │ │ │ │ ├── Array.h │ │ │ │ ├── ArrayBase.h │ │ │ │ ├── ArrayWrapper.h │ │ │ │ ├── Assign.h │ │ │ │ ├── AssignEvaluator.h │ │ │ │ ├── Assign_MKL.h │ │ │ │ ├── BandMatrix.h │ │ │ │ ├── Block.h │ │ │ │ ├── BooleanRedux.h │ │ │ │ ├── CommaInitializer.h │ │ │ │ ├── ConditionEstimator.h │ │ │ │ ├── CoreEvaluators.h │ │ │ │ ├── CoreIterators.h │ │ │ │ ├── CwiseBinaryOp.h │ │ │ │ ├── CwiseNullaryOp.h │ │ │ │ ├── CwiseTernaryOp.h │ │ │ │ ├── CwiseUnaryOp.h │ │ │ │ ├── CwiseUnaryView.h │ │ │ │ ├── DenseBase.h │ │ │ │ ├── DenseCoeffsBase.h │ │ │ │ ├── DenseStorage.h │ │ │ │ ├── Diagonal.h │ │ │ │ ├── DiagonalMatrix.h │ │ │ │ ├── DiagonalProduct.h │ │ │ │ ├── Dot.h │ │ │ │ ├── EigenBase.h │ │ │ │ ├── ForceAlignedAccess.h │ │ │ │ ├── Fuzzy.h │ │ │ │ ├── GeneralProduct.h │ │ │ │ ├── GenericPacketMath.h │ │ │ │ ├── GlobalFunctions.h │ │ │ │ ├── IO.h │ │ │ │ ├── IndexedView.h │ │ │ │ ├── Inverse.h │ │ │ │ ├── Map.h │ │ │ │ ├── MapBase.h │ │ │ │ ├── MathFunctions.h │ │ │ │ ├── MathFunctionsImpl.h │ │ │ │ ├── Matrix.h │ │ │ │ ├── MatrixBase.h │ │ │ │ ├── NestByValue.h │ │ │ │ ├── NoAlias.h │ │ │ │ ├── NumTraits.h │ │ │ │ ├── PartialReduxEvaluator.h │ │ │ │ ├── PermutationMatrix.h │ │ │ │ ├── PlainObjectBase.h │ │ │ │ ├── Product.h │ │ │ │ ├── ProductEvaluators.h │ │ │ │ ├── Random.h │ │ │ │ ├── Redux.h │ │ │ │ ├── Ref.h │ │ │ │ ├── Replicate.h │ │ │ │ ├── Reshaped.h │ │ │ │ ├── ReturnByValue.h │ │ │ │ ├── Reverse.h │ │ │ │ ├── Select.h │ │ │ │ ├── SelfAdjointView.h │ │ │ │ ├── SelfCwiseBinaryOp.h │ │ │ │ ├── Solve.h │ │ │ │ ├── SolveTriangular.h │ │ │ │ ├── SolverBase.h │ │ │ │ ├── StableNorm.h │ │ │ │ ├── StlIterators.h │ │ │ │ ├── Stride.h │ │ │ │ ├── Swap.h │ │ │ │ ├── Transpose.h │ │ │ │ ├── Transpositions.h │ │ │ │ ├── TriangularMatrix.h │ │ │ │ ├── VectorBlock.h │ │ │ │ ├── VectorwiseOp.h │ │ │ │ ├── Visitor.h │ │ │ │ ├── arch │ │ │ │ │ ├── AVX │ │ │ │ │ │ ├── Complex.h │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ ├── PacketMath.h │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ ├── AVX512 │ │ │ │ │ │ ├── Complex.h │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ ├── PacketMath.h │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ ├── AltiVec │ │ │ │ │ │ ├── Complex.h │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ ├── MatrixProduct.h │ │ │ │ │ │ ├── MatrixProductCommon.h │ │ │ │ │ │ ├── MatrixProductMMA.h │ │ │ │ │ │ └── PacketMath.h │ │ │ │ │ ├── CUDA │ │ │ │ │ │ └── Complex.h │ │ │ │ │ ├── Default │ │ │ │ │ │ ├── BFloat16.h │ │ │ │ │ │ ├── ConjHelper.h │ │ │ │ │ │ ├── GenericPacketMathFunctions.h │ │ │ │ │ │ ├── GenericPacketMathFunctionsFwd.h │ │ │ │ │ │ ├── Half.h │ │ │ │ │ │ ├── Settings.h │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ ├── GPU │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ ├── PacketMath.h │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ ├── HIP │ │ │ │ │ │ └── hcc │ │ │ │ │ │ │ └── math_constants.h │ │ │ │ │ ├── MSA │ │ │ │ │ │ ├── Complex.h │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ └── PacketMath.h │ │ │ │ │ ├── NEON │ │ │ │ │ │ ├── Complex.h │ │ │ │ │ │ ├── GeneralBlockPanelKernel.h │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ ├── PacketMath.h │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ ├── SSE │ │ │ │ │ │ ├── Complex.h │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ ├── PacketMath.h │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ ├── SVE │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ ├── PacketMath.h │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ ├── SYCL │ │ │ │ │ │ ├── InteropHeaders.h │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ ├── PacketMath.h │ │ │ │ │ │ ├── SyclMemoryModel.h │ │ │ │ │ │ └── TypeCasting.h │ │ │ │ │ └── ZVector │ │ │ │ │ │ ├── Complex.h │ │ │ │ │ │ ├── MathFunctions.h │ │ │ │ │ │ └── PacketMath.h │ │ │ │ ├── functors │ │ │ │ │ ├── AssignmentFunctors.h │ │ │ │ │ ├── BinaryFunctors.h │ │ │ │ │ ├── NullaryFunctors.h │ │ │ │ │ ├── StlFunctors.h │ │ │ │ │ ├── TernaryFunctors.h │ │ │ │ │ └── UnaryFunctors.h │ │ │ │ ├── products │ │ │ │ │ ├── GeneralBlockPanelKernel.h │ │ │ │ │ ├── GeneralMatrixMatrix.h │ │ │ │ │ ├── GeneralMatrixMatrixTriangular.h │ │ │ │ │ ├── GeneralMatrixMatrixTriangular_BLAS.h │ │ │ │ │ ├── GeneralMatrixMatrix_BLAS.h │ │ │ │ │ ├── GeneralMatrixVector.h │ │ │ │ │ ├── GeneralMatrixVector_BLAS.h │ │ │ │ │ ├── Parallelizer.h │ │ │ │ │ ├── SelfadjointMatrixMatrix.h │ │ │ │ │ ├── SelfadjointMatrixMatrix_BLAS.h │ │ │ │ │ ├── SelfadjointMatrixVector.h │ │ │ │ │ ├── SelfadjointMatrixVector_BLAS.h │ │ │ │ │ ├── SelfadjointProduct.h │ │ │ │ │ ├── SelfadjointRank2Update.h │ │ │ │ │ ├── TriangularMatrixMatrix.h │ │ │ │ │ ├── TriangularMatrixMatrix_BLAS.h │ │ │ │ │ ├── TriangularMatrixVector.h │ │ │ │ │ ├── TriangularMatrixVector_BLAS.h │ │ │ │ │ ├── TriangularSolverMatrix.h │ │ │ │ │ ├── TriangularSolverMatrix_BLAS.h │ │ │ │ │ └── TriangularSolverVector.h │ │ │ │ └── util │ │ │ │ │ ├── BlasUtil.h │ │ │ │ │ ├── ConfigureVectorization.h │ │ │ │ │ ├── Constants.h │ │ │ │ │ ├── DisableStupidWarnings.h │ │ │ │ │ ├── ForwardDeclarations.h │ │ │ │ │ ├── IndexedViewHelper.h │ │ │ │ │ ├── IntegralConstant.h │ │ │ │ │ ├── MKL_support.h │ │ │ │ │ ├── Macros.h │ │ │ │ │ ├── Memory.h │ │ │ │ │ ├── Meta.h │ │ │ │ │ ├── NonMPL2.h │ │ │ │ │ ├── ReenableStupidWarnings.h │ │ │ │ │ ├── ReshapedHelper.h │ │ │ │ │ ├── StaticAssert.h │ │ │ │ │ ├── SymbolicIndex.h │ │ │ │ │ └── XprHelper.h │ │ │ │ ├── Eigenvalues │ │ │ │ ├── ComplexEigenSolver.h │ │ │ │ ├── ComplexSchur.h │ │ │ │ ├── ComplexSchur_LAPACKE.h │ │ │ │ ├── EigenSolver.h │ │ │ │ ├── GeneralizedEigenSolver.h │ │ │ │ ├── GeneralizedSelfAdjointEigenSolver.h │ │ │ │ ├── HessenbergDecomposition.h │ │ │ │ ├── MatrixBaseEigenvalues.h │ │ │ │ ├── RealQZ.h │ │ │ │ ├── RealSchur.h │ │ │ │ ├── RealSchur_LAPACKE.h │ │ │ │ ├── SelfAdjointEigenSolver.h │ │ │ │ ├── SelfAdjointEigenSolver_LAPACKE.h │ │ │ │ └── Tridiagonalization.h │ │ │ │ ├── Geometry │ │ │ │ ├── AlignedBox.h │ │ │ │ ├── AngleAxis.h │ │ │ │ ├── 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 │ │ │ │ │ └── Geometry_SIMD.h │ │ │ │ ├── Householder │ │ │ │ ├── BlockHouseholder.h │ │ │ │ ├── Householder.h │ │ │ │ └── HouseholderSequence.h │ │ │ │ ├── IterativeLinearSolvers │ │ │ │ ├── BasicPreconditioners.h │ │ │ │ ├── BiCGSTAB.h │ │ │ │ ├── ConjugateGradient.h │ │ │ │ ├── IncompleteCholesky.h │ │ │ │ ├── IncompleteLUT.h │ │ │ │ ├── IterativeSolverBase.h │ │ │ │ ├── LeastSquareConjugateGradient.h │ │ │ │ └── SolveWithGuess.h │ │ │ │ ├── Jacobi │ │ │ │ └── Jacobi.h │ │ │ │ ├── KLUSupport │ │ │ │ └── KLUSupport.h │ │ │ │ ├── LU │ │ │ │ ├── Determinant.h │ │ │ │ ├── FullPivLU.h │ │ │ │ ├── InverseImpl.h │ │ │ │ ├── PartialPivLU.h │ │ │ │ ├── PartialPivLU_LAPACKE.h │ │ │ │ └── arch │ │ │ │ │ └── InverseSize4.h │ │ │ │ ├── MetisSupport │ │ │ │ └── MetisSupport.h │ │ │ │ ├── OrderingMethods │ │ │ │ ├── Amd.h │ │ │ │ ├── Eigen_Colamd.h │ │ │ │ └── Ordering.h │ │ │ │ ├── PaStiXSupport │ │ │ │ └── PaStiXSupport.h │ │ │ │ ├── PardisoSupport │ │ │ │ └── PardisoSupport.h │ │ │ │ ├── QR │ │ │ │ ├── ColPivHouseholderQR.h │ │ │ │ ├── ColPivHouseholderQR_LAPACKE.h │ │ │ │ ├── CompleteOrthogonalDecomposition.h │ │ │ │ ├── FullPivHouseholderQR.h │ │ │ │ ├── HouseholderQR.h │ │ │ │ └── HouseholderQR_LAPACKE.h │ │ │ │ ├── SPQRSupport │ │ │ │ └── SuiteSparseQRSupport.h │ │ │ │ ├── SVD │ │ │ │ ├── BDCSVD.h │ │ │ │ ├── JacobiSVD.h │ │ │ │ ├── JacobiSVD_LAPACKE.h │ │ │ │ ├── SVDBase.h │ │ │ │ └── UpperBidiagonalization.h │ │ │ │ ├── SparseCholesky │ │ │ │ ├── SimplicialCholesky.h │ │ │ │ └── SimplicialCholesky_impl.h │ │ │ │ ├── SparseCore │ │ │ │ ├── AmbiVector.h │ │ │ │ ├── CompressedStorage.h │ │ │ │ ├── ConservativeSparseSparseProduct.h │ │ │ │ ├── MappedSparseMatrix.h │ │ │ │ ├── SparseAssign.h │ │ │ │ ├── SparseBlock.h │ │ │ │ ├── SparseColEtree.h │ │ │ │ ├── SparseCompressedBase.h │ │ │ │ ├── SparseCwiseBinaryOp.h │ │ │ │ ├── SparseCwiseUnaryOp.h │ │ │ │ ├── SparseDenseProduct.h │ │ │ │ ├── SparseDiagonalProduct.h │ │ │ │ ├── SparseDot.h │ │ │ │ ├── SparseFuzzy.h │ │ │ │ ├── SparseMap.h │ │ │ │ ├── SparseMatrix.h │ │ │ │ ├── SparseMatrixBase.h │ │ │ │ ├── SparsePermutation.h │ │ │ │ ├── SparseProduct.h │ │ │ │ ├── SparseRedux.h │ │ │ │ ├── SparseRef.h │ │ │ │ ├── SparseSelfAdjointView.h │ │ │ │ ├── SparseSolverBase.h │ │ │ │ ├── SparseSparseProductWithPruning.h │ │ │ │ ├── SparseTranspose.h │ │ │ │ ├── SparseTriangularView.h │ │ │ │ ├── SparseUtil.h │ │ │ │ ├── SparseVector.h │ │ │ │ ├── SparseView.h │ │ │ │ └── TriangularSolver.h │ │ │ │ ├── SparseLU │ │ │ │ ├── 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 │ │ │ │ └── SparseQR.h │ │ │ │ ├── StlSupport │ │ │ │ ├── StdDeque.h │ │ │ │ ├── StdList.h │ │ │ │ ├── StdVector.h │ │ │ │ └── details.h │ │ │ │ ├── SuperLUSupport │ │ │ │ └── SuperLUSupport.h │ │ │ │ ├── UmfPackSupport │ │ │ │ └── UmfPackSupport.h │ │ │ │ ├── misc │ │ │ │ ├── Image.h │ │ │ │ ├── Kernel.h │ │ │ │ ├── RealSvd2x2.h │ │ │ │ ├── blas.h │ │ │ │ ├── lapack.h │ │ │ │ ├── lapacke.h │ │ │ │ └── lapacke_mangling.h │ │ │ │ └── plugins │ │ │ │ ├── ArrayCwiseBinaryOps.h │ │ │ │ ├── ArrayCwiseUnaryOps.h │ │ │ │ ├── BlockMethods.h │ │ │ │ ├── CommonCwiseBinaryOps.h │ │ │ │ ├── CommonCwiseUnaryOps.h │ │ │ │ ├── IndexedViewMethods.h │ │ │ │ ├── MatrixCwiseBinaryOps.h │ │ │ │ ├── MatrixCwiseUnaryOps.h │ │ │ │ └── ReshapedMethods.h │ │ ├── core │ │ │ ├── CameraStereoImages.h │ │ │ ├── Directory.h │ │ │ ├── EigenTypes.h │ │ │ ├── FPGA.h │ │ │ ├── FlannIndex.h │ │ │ ├── GFTT.h │ │ │ ├── Graph.h │ │ │ ├── GraphEdge.h │ │ │ ├── GraphVertex.h │ │ │ ├── HyperGraph.h │ │ │ ├── Link.h │ │ │ ├── Logger.h │ │ │ ├── Mapper.h │ │ │ ├── MotionEstimation.h │ │ │ ├── Node.h │ │ │ ├── Odometry.h │ │ │ ├── Optimizer.h │ │ │ ├── Parameters.h │ │ │ ├── Perf.h │ │ │ ├── Registration.h │ │ │ ├── SensorData.h │ │ │ ├── Stereo.h │ │ │ ├── StereoCameraModel.h │ │ │ ├── Transform.h │ │ │ ├── VWDictionary.h │ │ │ ├── VisualWord.h │ │ │ └── xThread.h │ │ ├── flann │ │ │ ├── all_indices.h │ │ │ ├── allocator.h │ │ │ ├── any.h │ │ │ ├── config.h │ │ │ ├── defines.h │ │ │ ├── dist.h │ │ │ ├── dynamic_bitset.h │ │ │ ├── flann.hpp │ │ │ ├── general.h │ │ │ ├── heap.h │ │ │ ├── kdtree_index.h │ │ │ ├── matrix.h │ │ │ ├── nn_index.h │ │ │ ├── params.h │ │ │ └── result_set.h │ │ ├── g2o │ │ │ └── SE3Gradient.h │ │ ├── octomap │ │ │ ├── AbstractOcTree.h │ │ │ ├── AbstractOccupancyOcTree.h │ │ │ ├── ColorOcTree.h │ │ │ ├── CountingOcTree.h │ │ │ ├── MCTables.h │ │ │ ├── OcTree.h │ │ │ ├── OcTreeBase.h │ │ │ ├── OcTreeBaseImpl.h │ │ │ ├── OcTreeBaseImpl.hxx │ │ │ ├── OcTreeDataNode.h │ │ │ ├── OcTreeDataNode.hxx │ │ │ ├── OcTreeIterator.hxx │ │ │ ├── OcTreeKey.h │ │ │ ├── OcTreeNode.h │ │ │ ├── OcTreeStamped.h │ │ │ ├── OccupancyOcTreeBase.h │ │ │ ├── OccupancyOcTreeBase.hxx │ │ │ ├── Pointcloud.h │ │ │ ├── ScanGraph.h │ │ │ ├── math │ │ │ │ ├── Pose6D.h │ │ │ │ ├── Quaternion.h │ │ │ │ ├── Utils.h │ │ │ │ └── Vector3.h │ │ │ ├── octomap.h │ │ │ ├── octomap_deprecated.h │ │ │ ├── octomap_types.h │ │ │ └── octomap_utils.h │ │ ├── opencv │ │ │ ├── CvLKStereo.h │ │ │ ├── CvORB.h │ │ │ └── CvSolvePnP.h │ │ └── rtabmap │ │ │ └── KITTI.h │ └── src │ │ ├── core │ │ ├── CameraStereoImages.cpp │ │ ├── Directory.cpp │ │ ├── FPGA.cpp │ │ ├── FlannIndex.cpp │ │ ├── GFTT.cpp │ │ ├── Graph.cpp │ │ ├── GraphEdge.cpp │ │ ├── GraphVertex.cpp │ │ ├── HyperGraph.cpp │ │ ├── Link.cpp │ │ ├── Logger.cpp │ │ ├── Mapper.cpp │ │ ├── MotionEstimation.cpp │ │ ├── Node.cpp │ │ ├── Odometry.cpp │ │ ├── Optimizer.cpp │ │ ├── Parameters.cpp │ │ ├── Perf.cpp │ │ ├── Registration.cpp │ │ ├── SensorData.cpp │ │ ├── Stereo.cpp │ │ ├── StereoCameraModel.cpp │ │ ├── Transform.cpp │ │ ├── VWDictionary.cpp │ │ ├── VisualWord.cpp │ │ ├── main.cpp │ │ └── xThread.cpp │ │ ├── g2o │ │ └── SE3Gradient.cpp │ │ ├── octomap │ │ ├── AbstractOcTree.cpp │ │ ├── AbstractOccupancyOcTree.cpp │ │ ├── ColorOcTree.cpp │ │ ├── CountingOcTree.cpp │ │ ├── OcTree.cpp │ │ ├── OcTreeNode.cpp │ │ ├── OcTreeStamped.cpp │ │ ├── Pointcloud.cpp │ │ ├── Pose6D.cpp │ │ ├── Quaternion.cpp │ │ ├── ScanGraph.cpp │ │ └── Vector3.cpp │ │ ├── opencv │ │ ├── CvLKStereo.cpp │ │ ├── CvORB.cpp │ │ └── CvSolvePnP.cpp │ │ └── rtabmap │ │ └── KITTI.cpp └── stereo_calib │ └── stereo_calib.cpp ├── title_600_450.png └── vivado ├── create_dvp.tcl └── create_fpga_top.tcl /README.md: -------------------------------------------------------------------------------- 1 | # U96-SLAM 2 | Visual SLAM on Ultra96-V2 3 | 4 | ![U96-SLAM](title_600_450.png) 5 | 6 | 7 | ## About 8 | U96-SLAM is an implementation of stereo-vision-based SLAM on Ultra96-V2. 9 | This repository contains all design files for this project. 10 | 11 | 12 | ## Article 13 | The article is published in Hackster web site that describes how to build the project in detail. 14 | 15 | Hackster [U96-SLAM](https://www.hackster.io/sdoira/visual-slam-on-ultra96-v2-9383d7) 16 | 17 | 18 | ## License 19 | The design files contained in the "src" directory are distributed under the following licenses. 20 | 21 | src 22 | ├─slam 23 | │ └─src/include 24 | │ ├─core ┄┄ MIT 25 | │ ├─octomap ┄┄ New BSD 26 | │ ├─flann ┄┄ BSD 27 | │ ├─rtabmap ┄┄ BSD 28 | │ └─opencv ┄┄ New BSD 29 | ├─StreoBM ┄┄ MIT 30 | ├─dvp ┄┄ MIT 31 | ├─capture_video ┄┄ MIT 32 | └─stereo_calib ┄┄ MIT 33 | 34 | The road scene image in the title is taken from the KITTI Dataset. 35 | 36 | -------------------------------------------------------------------------------- /bin/BOOT.BIN: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/bin/BOOT.BIN -------------------------------------------------------------------------------- /bin/StereoBM.elf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/bin/StereoBM.elf -------------------------------------------------------------------------------- /bin/boot.scr: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/bin/boot.scr -------------------------------------------------------------------------------- /bin/design_1_wrapper.bit: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/bin/design_1_wrapper.bit -------------------------------------------------------------------------------- /bin/design_1_wrapper.xsa: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/bin/design_1_wrapper.xsa -------------------------------------------------------------------------------- /bin/image.ub: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/bin/image.ub -------------------------------------------------------------------------------- /data/ref_rect_l.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/data/ref_rect_l.zip -------------------------------------------------------------------------------- /data/ref_rect_r.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/data/ref_rect_r.zip -------------------------------------------------------------------------------- /data/ref_xsbl_l.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/data/ref_xsbl_l.zip -------------------------------------------------------------------------------- /data/ref_xsbl_r.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/data/ref_xsbl_r.zip -------------------------------------------------------------------------------- /doc/chessboard.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/doc/chessboard.pdf -------------------------------------------------------------------------------- /doc/fpga/RegisterMap.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/doc/fpga/RegisterMap.xlsx -------------------------------------------------------------------------------- /doc/petalinux/linux.bif: -------------------------------------------------------------------------------- 1 | /* linux */ 2 | the_ROM_image: 3 | { 4 | [fsbl_config] a53_x64 5 | [bootloader] 6 | [pmufw_image] 7 | [destination_device=pl] 8 | [destination_cpu=a53-0, exception_level=el-3, trustzone] 9 | [destination_cpu=a53-0, exception_level=el-2] 10 | } 11 | -------------------------------------------------------------------------------- /doc/petalinux/myinit: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | cd ~/home/root 3 | ./run 4 | -------------------------------------------------------------------------------- /doc/petalinux/myinit.bb: -------------------------------------------------------------------------------- 1 | # 2 | # This file is the myapp-init recipe. 3 | # 4 | SUMMARY = "Simple myinit application" 5 | SECTION = "PETALINUX/apps" 6 | LICENSE = "MIT" 7 | LIC_FILES_CHKSUM = "file://${COMMON_LICENSE_DIR}/MIT;md5=0835ade698e0bcf8506ecda2f7b4f302" 8 | 9 | SRC_URI = "file://myinit \ 10 | " 11 | 12 | S = "${WORKDIR}" 13 | 14 | FILESEXTRAPATHS_prepend := "${THISDIR}/files:" 15 | inherit update-rc.d 16 | INITSCRIPT_NAME = "myinit" 17 | INITSCRIPT_PARAMS = "start 99 S ." 18 | 19 | do_install() { 20 | install -d ${D}${sysconfdir}/init.d 21 | install -m 0755 ${S}/myinit ${D}${sysconfdir}/init.d/myinit 22 | } 23 | 24 | FILES_${PN} += "${sysconfdir}/*" 25 | 26 | -------------------------------------------------------------------------------- /doc/petalinux/pl.dtsi: -------------------------------------------------------------------------------- 1 | /* 2 | * CAUTION: This file is automatically generated by Xilinx. 3 | * Version: 4 | * Today is: Fri Mar 24 05:07:09 2023 5 | */ 6 | 7 | 8 | / { 9 | amba_pl: amba_pl@0 { 10 | #address-cells = <2>; 11 | #size-cells = <2>; 12 | compatible = "simple-bus"; 13 | ranges ; 14 | dvp_0: dvp@a0000000 { 15 | clock-names = "s00_axi_aclk", "m00_axi_aclk"; 16 | clocks = <&zynqmp_clk 71>, <&zynqmp_clk 71>; 17 | compatible = "xlnx,dvp-1.0"; 18 | interrupt-names = "intr"; 19 | interrupt-parent = <&gic>; 20 | interrupts = <0 89 4>; 21 | reg = <0x0 0xa0000000 0x0 0x10000>; 22 | xlnx,m00-axi-addr-width = <0x20>; 23 | xlnx,m00-axi-aruser-width = <0x0>; 24 | xlnx,m00-axi-awuser-width = <0x0>; 25 | xlnx,m00-axi-burst-len = <0x10>; 26 | xlnx,m00-axi-buser-width = <0x0>; 27 | xlnx,m00-axi-data-width = <0x20>; 28 | xlnx,m00-axi-id-width = <0x1>; 29 | xlnx,m00-axi-ruser-width = <0x0>; 30 | xlnx,m00-axi-target-slave-base-addr = <0x40000000>; 31 | xlnx,m00-axi-wuser-width = <0x0>; 32 | }; 33 | misc_clk_0: misc_clk_0 { 34 | #clock-cells = <0>; 35 | clock-frequency = <200000000>; 36 | compatible = "fixed-clock"; 37 | }; 38 | misc_clk_1: misc_clk_1 { 39 | #clock-cells = <0>; 40 | clock-frequency = <1500000000>; 41 | compatible = "fixed-clock"; 42 | }; 43 | }; 44 | }; 45 | -------------------------------------------------------------------------------- /doc/petalinux/run: -------------------------------------------------------------------------------- 1 | rm *.csv 2 | rm *.bmp 3 | rm *.png 4 | rm *.jpg 5 | rm *.txt 6 | rm -rf work 7 | echo StereoBM.elf > /sys/class/remoteproc/remoteproc0/firmware 8 | echo start > /sys/class/remoteproc/remoteproc0/state 9 | #/lib/firmware/slam.elf -app "STEREO_CAPTURE" -lc "calib_left.yml" -rc "calib_right.yml" 10 | #/lib/firmware/slam.elf -app "FRAME_GRABBER" 11 | #/lib/firmware/slam.elf -app "SLAM_BATCH" -dir "kitti/sequences/00" -l "image_0" -r "image_1" -t "times.txt" -gt "../../poses/00.txt" -lc "calib.txt" -n 100 12 | /lib/firmware/slam.elf -app "SLAM_REALTIME" -lc "calib_left.yml" -rc "calib_right.yml" 13 | shutdown -h now 14 | -------------------------------------------------------------------------------- /doc/petalinux/system-user.dtsi: -------------------------------------------------------------------------------- 1 | /include/ "system-conf.dtsi" 2 | / { 3 | reserved-memory { 4 | #address-cells = <2>; 5 | #size-cells = <2>; 6 | ranges; 7 | rproc_0_dma: rproc@0x6ed00000 { 8 | no-map; 9 | compatible = "shared-dma-pool"; 10 | reg = <0x0 0x6ed00000 0x0 0x00100000>; 11 | }; 12 | rproc_0_reserved: rproc@0x5ed00000 { 13 | no-map; 14 | reg = <0x0 0x5ed00000 0x0 0x10000000>; 15 | }; 16 | }; 17 | zynqmp-rpu { 18 | compatible = "xlnx,zynqmp-r5-remoteproc-1.0"; 19 | #address-cells = <2>; 20 | #size-cells = <2>; 21 | ranges; 22 | core_conf = "split"; 23 | r5_0: r5@0 { 24 | #address-cells = <2>; 25 | #size-cells = <2>; 26 | ranges; 27 | memory-region = <&rproc_0_reserved>, <&rproc_0_dma>; 28 | pnode-id = <0x7>; 29 | mboxes = <&ipi_mailbox_rpu0 0>, <&ipi_mailbox_rpu0 1>; 30 | mbox-names = "tx", "rx"; 31 | tcm_0_a: tcm_0@0 { 32 | reg = <0x0 0xFFE00000 0x0 0x10000>; 33 | pnode-id = <0xf>; 34 | }; 35 | tcm_0_b: tcm_0@1 { 36 | reg = <0x0 0xFFE20000 0x0 0x10000>; 37 | pnode-id = <0x10>; 38 | }; 39 | }; 40 | }; 41 | zynqmp_ipi1 { 42 | compatible = "xlnx,zynqmp-ipi-mailbox"; 43 | interrupt-parent = <&gic>; 44 | interrupts = <0 29 4>; 45 | xlnx,ipi-id = <7>; 46 | #address-cells = <1>; 47 | #size-cells = <1>; 48 | ranges; 49 | /* APU<->RPU0 IPI mailbox controller */ 50 | ipi_mailbox_rpu0: mailbox@ff90000 { 51 | reg = <0xff990600 0x20>, 52 | <0xff990620 0x20>, 53 | <0xff9900c0 0x20>, 54 | <0xff9900e0 0x20>; 55 | reg-names = "local_request_region", 56 | "local_response_region", 57 | "remote_request_region", 58 | "remote_response_region"; 59 | #mbox-cells = <1>; 60 | xlnx,ipi-id = <1>; 61 | }; 62 | }; 63 | chosen { 64 | bootargs = "console=ttyPS0,115200 root=/dev/mmcblk0p2 rw earlyprintk rootfstype=ext4 rootwait uio_pdrv_genirq.of_id=generic-uio devtmpfs.mount=1 earlycon"; 65 | }; 66 | }; 67 | 68 | &dvp_0 { 69 | compatible = "generic-uio"; 70 | }; 71 | -------------------------------------------------------------------------------- /src/StereoBM/src/fpga.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/src/StereoBM/src/fpga.c -------------------------------------------------------------------------------- /src/StereoBM/src/main.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2020 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | //============================================================================= 6 | //! 7 | //! @file main.h 8 | //! 9 | //! Header file for main.c 10 | //! 11 | //! 
12 | //! MODIFICATION HISTORY:
13 | //!
14 | //! Ver   Who  Date       Changes
15 | //! ----- ---- ---------- -----------------------------------------------------
16 | //! 1.0   sd   2020.12.07 First release
17 | //!
18 | //============================================================================= 19 | #ifndef MAIN_H 20 | #define MAIN_H 21 | 22 | #include "xil_printf.h" 23 | #include "sleep.h" 24 | #include "xil_cache.h" 25 | 26 | #include "uart.h" 27 | #include "i2c.h" 28 | #include "ov5640.h" 29 | #include "csi.h" 30 | #include "fpga.h" 31 | #include "xusb_main.h" 32 | #include "lsm9ds1.h" 33 | 34 | //! Command structure 35 | struct HWTP_CMD { 36 | char parm[10][16]; 37 | int num; 38 | }; 39 | 40 | //! @name Buffer Address for Bank A/B/C 41 | //! @{ 42 | #define IMG_BUF_A 0x40000000 43 | #define IMG_BUF_B 0x41000000 44 | #define IMG_BUF_C 0x42000000 45 | //! @} 46 | 47 | 48 | //============================================================================= 49 | // Function Prototypes 50 | //============================================================================= 51 | void PL_IntrHandler (void *CallBackRef); 52 | s32 SetupInterruptSystem (u16 IntcDeviceID, void *IntcPtr); 53 | s32 PL_SetupInterrupt ( 54 | XScuGic_Config *IntcConfig, 55 | void *IntcPtr, 56 | struct Usb_DevData *UsbInstance 57 | ); 58 | void AssertCallBack (const char8 *File, s32 Line); 59 | int main (void); 60 | 61 | // Application level functions 62 | void App_UsbGrabber (struct REMOTE_SETTING *remoteSetting); 63 | void App_9DofHwTest (void); 64 | 65 | // Command related functions 66 | void Prompt (void); 67 | void CommandProcess (void); 68 | void CommandReceive (char* buf, int len); 69 | void CommandAnalyze (struct HWTP_CMD *cmd); 70 | void CommandExecute (struct HWTP_CMD cmd); 71 | 72 | // Helper functions 73 | void hex_to_num (char* hex, unsigned long* num); 74 | void dec_to_num (char* dec, unsigned long* num); 75 | void set_bit (unsigned int *reg, int bit_mask); 76 | void clear_bit (unsigned int *reg, int bit_mask); 77 | int get_bit (unsigned int reg, int bit_mask); 78 | 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /src/StereoBM/src/ov5640.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/src/StereoBM/src/ov5640.c -------------------------------------------------------------------------------- /src/StereoBM/src/uart.c: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2020 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | //============================================================================= 6 | //! 7 | //! @file uart.c 8 | //! 9 | //! C source for PS UART module. 10 | //! 11 | //! 
12 | //! MODIFICATION HISTORY:
13 | //!
14 | //! Ver   Who  Date       Changes
15 | //! ----- ---- ---------- -----------------------------------------------------
16 | //! 1.0   sd   2020.12.07 First release
17 | //!
18 | //============================================================================= 19 | #include "uart.h" 20 | 21 | //============================================================================= 22 | //! UART Get Char 23 | //----------------------------------------------------------------------------- 24 | //! @return One byte of received data 25 | //----------------------------------------------------------------------------- 26 | //! @brief This function waits until there is any data received then returns 27 | //! 1 byte of receive data. 28 | //============================================================================= 29 | unsigned char uart_getc(void){ 30 | // Wait until there is data 31 | while (!XUartPs_IsReceiveData(UART_BASEADDR)); 32 | 33 | unsigned char RecvChar = XUartPs_ReadReg(UART_BASEADDR, XUARTPS_FIFO_OFFSET); 34 | 35 | return RecvChar; 36 | } 37 | 38 | //============================================================================= 39 | //! UART Get Char2 40 | //----------------------------------------------------------------------------- 41 | //! @param *data Recieve data is stored here 42 | //----------------------------------------------------------------------------- 43 | //! @return "1" if data is received, else "-1" 44 | //----------------------------------------------------------------------------- 45 | //! @brief Non-blocking variant of uart_getc function. This function returns 46 | //! immediately regardless of there is any data or not. 47 | //============================================================================= 48 | // non-blocking type 49 | int uart_getc2(unsigned char* data){ 50 | if (XUartPs_IsReceiveData(UART_BASEADDR)) { 51 | *data = XUartPs_ReadReg(UART_BASEADDR, XUARTPS_FIFO_OFFSET); 52 | return 1; 53 | } else { 54 | return -1; 55 | } 56 | } 57 | 58 | //============================================================================= 59 | //! UART Initialize 60 | //----------------------------------------------------------------------------- 61 | //! @brief This function initializes UART module 62 | //============================================================================= 63 | void uart_init(void) { 64 | unsigned int CntrlRegister = XUartPs_ReadReg(UART_BASEADDR, XUARTPS_CR_OFFSET); 65 | 66 | /// Enable TX and RX for the device 67 | XUartPs_WriteReg( 68 | UART_BASEADDR, 69 | XUARTPS_CR_OFFSET, 70 | ((CntrlRegister & ~XUARTPS_CR_EN_DIS_MASK) | XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN) 71 | ); 72 | } 73 | 74 | -------------------------------------------------------------------------------- /src/StereoBM/src/uart.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2020 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | //============================================================================= 6 | //! 7 | //! @file uart.h 8 | //! 9 | //! Header file for uart.c 10 | //! 11 | //! 
12 | //! MODIFICATION HISTORY:
13 | //!
14 | //! Ver   Who  Date       Changes
15 | //! ----- ---- ---------- -----------------------------------------------------
16 | //! 1.0   sd   2020.12.07 First release
17 | //!
18 | //============================================================================= 19 | #ifndef UART_H 20 | #define UART_H 21 | 22 | #include "xparameters.h" 23 | #include "xil_printf.h" 24 | #include "xuartps_hw.h" 25 | #include "xil_types.h" 26 | 27 | 28 | //============================================================================= 29 | // Define 30 | //============================================================================= 31 | #define UART_BASEADDR XPAR_XUARTPS_1_BASEADDR 32 | #define UART_CLOCK_HZ XPAR_XUARTPS_1_CLOCK_HZ 33 | 34 | 35 | //============================================================================= 36 | // Function Prototypes 37 | //============================================================================= 38 | unsigned char uart_getc(void); 39 | int uart_getc2(unsigned char* data); 40 | void uart_init(void); 41 | 42 | #endif // UART_H 43 | -------------------------------------------------------------------------------- /src/StereoBM/src/xusb_main.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2020 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | //============================================================================= 6 | //! 7 | //! @file xusb_main.h 8 | //! 9 | //! Header file for xusb_main.c 10 | //! 11 | //! 
12 | //! MODIFICATION HISTORY:
13 | //!
14 | //! Ver   Who  Date       Changes
15 | //! ----- ---- ---------- -----------------------------------------------------
16 | //! 1.0   sd   2020.12.07 First release
17 | //!
18 | //============================================================================= 19 | #ifndef XUSB_MAIN_H 20 | #define XUSB_MAIN_H 21 | 22 | #include "xparameters.h" 23 | #include "xil_printf.h" 24 | #include "sleep.h" 25 | #include 26 | #include "xusb_wrapper.h" 27 | #include "xil_exception.h" 28 | #include "xscugic.h" 29 | 30 | #include "xusb_ch9_video.h" 31 | #include "uart.h" 32 | #include "main.h" 33 | #include "fpga.h" 34 | #include "Parameters.h" 35 | 36 | //============================================================================= 37 | // Constant Definitions 38 | //============================================================================= 39 | #define MEMORY_SIZE (640 * 480 * 4 + 1024) 40 | u8 Buffer[MEMORY_SIZE] __attribute__ ((aligned(64))); 41 | 42 | #define CH9_DEBUG 43 | 44 | //============================================================================= 45 | // Function Prototypes 46 | //============================================================================= 47 | int Xusb_Init (void); 48 | int Xusb_Main (struct REMOTE_SETTING *remoteSetting); 49 | void IntrRxHandler (void *CallBackRef, u32 RequestedBytes, u32 BytesTxed); 50 | void BulkInHandler (void *CallBackRef, u32 RequestedBytes, u32 BytesTxed); 51 | s32 Xusb_SetupInterrupt (void *IntcPtr); 52 | void Xusb_ReceiveData ( 53 | struct UVC_APP_DATA *app_data, 54 | struct REMOTE_SETTING *remoteSetting, 55 | u8 **data_ptr); 56 | 57 | 58 | //============================================================================= 59 | // Variable Definitions 60 | //============================================================================= 61 | struct Usb_DevData UsbInstance; 62 | extern struct UVC_INFO uvc_info; 63 | extern struct XUSB_TX_INFO tx_info; 64 | 65 | Usb_Config *UsbConfigPtr; 66 | 67 | XScuGic InterruptController; // Interrupt controller instance 68 | 69 | #define INTC_DEVICE_ID XPAR_SCUGIC_SINGLE_DEVICE_ID 70 | #define USB_INT_ID XPAR_XUSBPS_0_INTR 71 | 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /src/dvp/rtl/clkx.v: -------------------------------------------------------------------------------- 1 | //========================================================================== 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //========================================================================== 5 | `timescale 1ns / 1ps 6 | 7 | //========================================================================== 8 | // Conveys an active-high pulse signal across different clock domains. 9 | //-------------------------------------------------------------------------- 10 | // clk1 can be higher than clk2. 11 | //========================================================================== 12 | module clkx ( 13 | rst_n, 14 | clk1, in, 15 | clk2, out 16 | ); 17 | 18 | 19 | //========================================================================== 20 | // Port Declaration 21 | //========================================================================== 22 | // Reset/Clock 23 | input rst_n; 24 | 25 | // Clock Domain 1 26 | input clk1, in; 27 | 28 | // Clock Domain 2 29 | input clk2; 30 | output out; 31 | 32 | 33 | //========================================================================== 34 | // Reg/Wire Declaration 35 | //========================================================================== 36 | reg in_r1, in_r2, in_r3; 37 | 38 | 39 | //========================================================================== 40 | // Clock Domain Crossing 41 | //========================================================================== 42 | always @(negedge rst_n or posedge clk1) begin 43 | if (~rst_n) begin 44 | in_r1 <= 1'b0; 45 | end 46 | else begin 47 | if (in_r3) begin 48 | in_r1 <= 1'b0; 49 | end 50 | else if (in) begin 51 | in_r1 <= 1'b1; 52 | end 53 | end 54 | end 55 | 56 | always @(negedge rst_n or posedge clk2) begin 57 | if (~rst_n) begin 58 | in_r2 <= 1'b0; 59 | end 60 | else begin 61 | if (in_r3) begin 62 | in_r2 <= 1'b0; 63 | end 64 | else if (in_r1) begin 65 | in_r2 <= 1'b1; 66 | end 67 | end 68 | end 69 | 70 | always @(negedge rst_n or posedge clk2) begin 71 | if (~rst_n) begin 72 | in_r3 <= 1'b0; 73 | end 74 | else begin 75 | if (in_r3) begin 76 | in_r3 <= 1'b0; 77 | end 78 | else if (in_r2) begin 79 | in_r3 <= 1'b1; 80 | end 81 | end 82 | end 83 | 84 | assign out = in_r3; 85 | 86 | 87 | endmodule 88 | -------------------------------------------------------------------------------- /src/dvp/rtl/clkx2.v: -------------------------------------------------------------------------------- 1 | //========================================================================== 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //========================================================================== 5 | `timescale 1ns / 1ps 6 | 7 | //========================================================================== 8 | // Conveys a high-active pulse signal across different clock domains. 9 | //-------------------------------------------------------------------------- 10 | // This module assumes that clk2 is higher than clk1. 11 | //========================================================================== 12 | module clkx2 ( 13 | rst_n, 14 | clk1, in, 15 | clk2, out 16 | ); 17 | 18 | 19 | //========================================================================== 20 | // Port Declaration 21 | //========================================================================== 22 | // Reset/Clock 23 | input rst_n; 24 | 25 | // Clock Domain 1 26 | input clk1, in; 27 | 28 | // Clock Domain 2 29 | input clk2; 30 | output out; 31 | 32 | 33 | //========================================================================== 34 | // Reg/Wire Declaration 35 | //========================================================================== 36 | reg [2:0] in_r; 37 | 38 | 39 | //========================================================================== 40 | // Clock Domain Crossing 41 | //========================================================================== 42 | always @(negedge rst_n or posedge clk2) begin 43 | if (~rst_n) begin 44 | in_r <= 3'b0; 45 | end 46 | else begin 47 | in_r <= {in_r[1:0], in}; 48 | end 49 | end 50 | 51 | assign out = ~in_r[2] & in_r[1]; 52 | 53 | 54 | endmodule 55 | -------------------------------------------------------------------------------- /src/dvp/rtl/rect_intp_fifo.v: -------------------------------------------------------------------------------- 1 | //========================================================================== 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //========================================================================== 5 | `timescale 1 ns / 1 ps 6 | 7 | module rect_intp_fifo ( 8 | // Global Control 9 | rst_n, clk, 10 | 11 | // FIFO I/F 12 | fifo_wr, 13 | fifo_din, 14 | fifo_rd, 15 | fifo_dout 16 | ); 17 | 18 | parameter W = 28; 19 | parameter D = 2; 20 | parameter DD = (1 << D); 21 | 22 | 23 | //========================================================================== 24 | // Port Declaration 25 | //========================================================================== 26 | // Global Control 27 | input rst_n, clk; 28 | 29 | // FIFO I/F 30 | input fifo_wr; 31 | input [W-1:0] fifo_din; 32 | input fifo_rd; 33 | output [W-1:0] fifo_dout; 34 | 35 | 36 | //========================================================================== 37 | // Reg/Wire Declaration 38 | //========================================================================== 39 | // Loop Index 40 | integer i; 41 | 42 | // Simple FIFO 43 | reg [D-1:0] wr_ptr; 44 | reg [W-1:0] store[DD-1:0]; 45 | reg [D-1:0] rd_ptr; 46 | wire [W-1:0] fifo_dout; 47 | 48 | 49 | //========================================================================== 50 | // Simple FIFO 51 | //========================================================================== 52 | always @(negedge rst_n or posedge clk) begin 53 | if (~rst_n) begin 54 | wr_ptr <= {D{1'b0}}; 55 | end 56 | else begin 57 | if (fifo_wr) begin 58 | wr_ptr <= wr_ptr + 1'b1; 59 | end 60 | end 61 | end 62 | 63 | always @(negedge rst_n or posedge clk) begin 64 | if (~rst_n) begin 65 | for (i = 0; i < DD; i = i + 1) begin 66 | store[i] <= {W{1'b0}}; 67 | end 68 | end 69 | else begin 70 | if (fifo_wr) begin 71 | store[wr_ptr] <= fifo_din; 72 | end 73 | end 74 | end 75 | 76 | always @(negedge rst_n or posedge clk) begin 77 | if (~rst_n) begin 78 | rd_ptr <= {D{1'b1}}; 79 | end 80 | else begin 81 | if (fifo_rd) begin 82 | rd_ptr <= rd_ptr + 1'b1; 83 | end 84 | end 85 | end 86 | 87 | assign fifo_dout = store[rd_ptr]; 88 | 89 | 90 | endmodule 91 | -------------------------------------------------------------------------------- /src/dvp/rtl/rect_rmp.v: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/src/dvp/rtl/rect_rmp.v -------------------------------------------------------------------------------- /src/dvp/rtl/simple_fifo.v: -------------------------------------------------------------------------------- 1 | //========================================================================== 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //========================================================================== 5 | `timescale 1 ns / 1 ps 6 | 7 | module simple_fifo # ( 8 | parameter integer W = 10, 9 | parameter integer D = 2 10 | ) ( 11 | // Global Control 12 | rst_n, clk, 13 | 14 | // FIFO I/F 15 | wr, 16 | wrdata, 17 | rd, 18 | rddata, 19 | 20 | // Status 21 | empty, 22 | full, 23 | data_count 24 | ); 25 | 26 | 27 | //========================================================================== 28 | // Parameters 29 | //========================================================================== 30 | parameter integer MAX_COUNT = (1'b1 << D); 31 | 32 | 33 | //========================================================================== 34 | // Port Declaration 35 | //========================================================================== 36 | // Global Control 37 | input rst_n, clk; 38 | 39 | // FIFO I/F 40 | input wr; 41 | input [W-1:0] wrdata; 42 | input rd; 43 | output [W-1:0] rddata; 44 | 45 | // Status 46 | output empty, full; 47 | output [D:0] data_count; 48 | 49 | 50 | //========================================================================== 51 | // Reg/Wire Declaration 52 | //========================================================================== 53 | integer i; 54 | reg [W-1:0] sftr[MAX_COUNT-1:0]; 55 | reg [D-1:0] rdaddr; 56 | wire [W-1:0] rddata; 57 | reg [D:0] data_count; 58 | wire empty, full; 59 | 60 | 61 | //========================================================================== 62 | // FIFO Implementation 63 | //========================================================================== 64 | always @(negedge rst_n or posedge clk) begin 65 | if (~rst_n) begin 66 | for (i = 0; i < MAX_COUNT; i = i + 1) begin 67 | sftr[i] <= {W-1{1'b0}}; 68 | end 69 | end 70 | else begin 71 | if (wr) begin 72 | // shift register operation 73 | for (i = 1; i < MAX_COUNT; i = i + 1) begin 74 | sftr[i] <= sftr[i-1]; 75 | end 76 | sftr[0] = wrdata; 77 | end 78 | end 79 | end 80 | 81 | always @(negedge rst_n or posedge clk) begin 82 | if (~rst_n) begin 83 | rdaddr <= {D{1'b1}}; // points the last 84 | end 85 | else begin 86 | if (wr & ~rd) begin 87 | rdaddr <= rdaddr + 1'b1; 88 | end 89 | else if (rd & ~wr) begin 90 | rdaddr <= rdaddr - 1'b1; 91 | end 92 | end 93 | end 94 | 95 | assign rddata[W-1:0] = sftr[rdaddr]; 96 | 97 | always @(negedge rst_n or posedge clk) begin 98 | if (~rst_n) begin 99 | data_count <= {D+1{1'b0}}; 100 | end 101 | else begin 102 | if (wr & ~rd) begin 103 | data_count <= data_count + 1'b1; 104 | end 105 | else if (rd & ~wr) begin 106 | data_count <= data_count - 1'b1; 107 | end 108 | end 109 | end 110 | 111 | assign empty = (data_count == 0); 112 | assign full = (data_count == MAX_COUNT); 113 | 114 | 115 | endmodule 116 | -------------------------------------------------------------------------------- /src/dvp/sim/dummy_bm.v: -------------------------------------------------------------------------------- 1 | 2 | `timescale 1 ns / 1 ps 3 | 4 | module dummy_bm ( 5 | // Global Control 6 | rst_n, clk, 7 | 8 | // Internal Bus I/F 9 | ibus_cs, 10 | ibus_wr, 11 | ibus_addr_7_2, 12 | ibus_wrdata, 13 | ibus_rddata, 14 | 15 | // Control 16 | xsbl_done, 17 | bm_done, 18 | xsbl_fcnt, 19 | bm_fcnt, 20 | enb, 21 | 22 | // DDR Read Arbiter I/F 23 | drd_req, 24 | drd_ack, 25 | drd_vin, 26 | drd_din, 27 | drd_vout, 28 | drd_dout, 29 | 30 | // DDR Write Arbiter I/F 31 | dwr_req, 32 | dwr_ack, 33 | dwr_vout, 34 | dwr_dout, 35 | dwr_strb 36 | ); 37 | 38 | 39 | //========================================================================== 40 | // Port Declaration 41 | //========================================================================== 42 | // Global Control 43 | input rst_n, clk; 44 | 45 | // Internal Bus I/F 46 | input ibus_cs; 47 | input ibus_wr; 48 | input [5:0] ibus_addr_7_2; 49 | input [31:0] ibus_wrdata; 50 | output [31:0] ibus_rddata; 51 | 52 | // Control 53 | input xsbl_done, bm_done; 54 | input [3:0] xsbl_fcnt; 55 | output [3:0] bm_fcnt; 56 | output enb; 57 | 58 | // DDR Read Arbiter I/F 59 | output drd_req; 60 | input drd_ack; 61 | input drd_vin; 62 | input [31:0] drd_din; 63 | output drd_vout; 64 | output [31:0] drd_dout; 65 | 66 | // DDR Write Arbiter I/F 67 | output dwr_req; 68 | input dwr_ack; 69 | output dwr_vout; 70 | output [31:0] dwr_dout; 71 | output [3:0] dwr_strb; 72 | 73 | 74 | //========================================================================== 75 | // Reg/Wire Declaration 76 | //========================================================================== 77 | reg [8:0] lr_addr_a, lr_addr_b; 78 | wire [6:0] sad_addr_a, sad_addr_b; 79 | wire [9:0] sad_wdt; 80 | wire [8:0] sad_hgt; 81 | wire bm_start; 82 | reg bm_sim_start; 83 | reg [8:0] disp2_addr_a, disp2_addr_b; 84 | reg [8:0] hgt; 85 | reg [9:0] wdt; 86 | 87 | 88 | //========================================================================== 89 | // Dummy Output 90 | //========================================================================== 91 | assign ibus_rddata = 32'b0; 92 | assign bm_fcnt = 4'b0; 93 | assign enb = 1'b0; 94 | assign drd_req = 1'b0; 95 | assign drd_vout = 1'b0; 96 | assign drd_dout = 32'b0; 97 | assign dwr_req = 1'b0; 98 | assign dwr_vout = 1'b0; 99 | assign dwr_dout = 32'b0; 100 | assign dwr_strb = 4'hF; 101 | 102 | 103 | endmodule 104 | -------------------------------------------------------------------------------- /src/dvp/sim/dummy_gftt.v: -------------------------------------------------------------------------------- 1 | 2 | `timescale 1 ns / 1 ps 3 | 4 | module dummy_gftt ( 5 | // Global Control 6 | rst_n, clk, 7 | 8 | // Internal Bus I/F 9 | ibus_cs, 10 | ibus_wr, 11 | ibus_addr_7_2, 12 | ibus_wrdata, 13 | ibus_rddata, 14 | 15 | // Control 16 | rect_done, 17 | gftt_done, 18 | rect_fcnt, 19 | gftt_fcnt, 20 | 21 | // DDR Read Arbiter I/F 22 | drd_req, 23 | drd_ack, 24 | drd_vin, 25 | drd_din, 26 | drd_vout, 27 | drd_dout, 28 | 29 | // DDR Write Arbiter I/F 30 | dwr_req, 31 | dwr_ack, 32 | dwr_vout, 33 | dwr_dout, 34 | dwr_strb 35 | ); 36 | 37 | 38 | //========================================================================== 39 | // Port Declaration 40 | //========================================================================== 41 | // Global Control 42 | input rst_n, clk; 43 | 44 | // Internal Bus I/F 45 | input ibus_cs; 46 | input ibus_wr; 47 | input [5:0] ibus_addr_7_2; 48 | input [31:0] ibus_wrdata; 49 | output [31:0] ibus_rddata; 50 | 51 | // Control 52 | input rect_done; 53 | input gftt_done; 54 | input [3:0] rect_fcnt; 55 | output [3:0] gftt_fcnt; 56 | 57 | // DDR Read Arbiter I/F 58 | output drd_req; 59 | input drd_ack; 60 | input drd_vin; 61 | input [31:0] drd_din; 62 | output drd_vout; 63 | output [31:0] drd_dout; 64 | 65 | // DDR Write Arbiter I/F 66 | output dwr_req; 67 | input dwr_ack; 68 | output dwr_vout; 69 | output [31:0] dwr_dout; 70 | output [3:0] dwr_strb; 71 | 72 | 73 | //========================================================================== 74 | // Reg/Wire Declaration 75 | //========================================================================== 76 | reg gftt_sim_start; 77 | 78 | 79 | //========================================================================== 80 | // Dummy Output 81 | //========================================================================== 82 | assign ibus_rddata = 32'b0; 83 | assign gftt_fcnt = 4'b0; 84 | assign drd_req = 1'b0; 85 | assign drd_vout = 1'b0; 86 | assign drd_dout = 32'b0; 87 | assign dwr_req = 1'b0; 88 | assign dwr_vout = 1'b0; 89 | assign dwr_dout = 32'b0; 90 | assign dwr_strb = 4'hF; 91 | 92 | 93 | endmodule 94 | -------------------------------------------------------------------------------- /src/dvp/sim/dummy_rect.v: -------------------------------------------------------------------------------- 1 | 2 | `timescale 1 ns / 1 ps 3 | 4 | module dummy_rect ( 5 | // Global Control 6 | rst_n, clk, 7 | 8 | // Internal Bus I/F 9 | ibus_cs, 10 | ibus_wr, 11 | ibus_addr_7_2, 12 | ibus_wrdata, 13 | ibus_rddata, 14 | 15 | // Control 16 | rect_done, 17 | rect_fcnt, 18 | bm_done, 19 | bm_enb, 20 | 21 | // Sensor Input 22 | pclk, 23 | vsync, 24 | href, 25 | d_l, 26 | d_r, 27 | 28 | // DDR I/F 29 | ddr_req, 30 | ddr_ack, 31 | ddr_dout, 32 | ddr_strb, 33 | ddr_vout 34 | ); 35 | 36 | 37 | //========================================================================== 38 | // Port Declaration 39 | //========================================================================== 40 | // Global Control 41 | input rst_n, clk; 42 | 43 | // Internal Bus I/F 44 | input ibus_cs; 45 | input ibus_wr; 46 | input [5:0] ibus_addr_7_2; 47 | input [31:0] ibus_wrdata; 48 | output [31:0] ibus_rddata; 49 | 50 | // Control 51 | input rect_done; 52 | output [3:0] rect_fcnt; 53 | input bm_done; 54 | input bm_enb; 55 | 56 | // Sensor Input 57 | input pclk, vsync, href; 58 | input [7:0] d_l, d_r; 59 | 60 | // DDR I/F 61 | output ddr_req; 62 | input ddr_ack; 63 | output [31:0] ddr_dout; 64 | output [3:0] ddr_strb; 65 | output ddr_vout; 66 | 67 | 68 | //========================================================================== 69 | // Reg/Wire Declaration 70 | //========================================================================== 71 | reg [9:0] base_a, base_b; 72 | 73 | 74 | //========================================================================== 75 | // Dummy Output 76 | //========================================================================== 77 | assign ibus_rddata[31:0] = 32'b0; 78 | assign rect_fcnt[3:0] = 4'b0; 79 | assign ddr_req = 1'b0; 80 | assign ddr_dout[31:0] = 32'b0; 81 | assign ddr_strb[3:0] = 4'hF; 82 | assign ddr_vout = 1'b0; 83 | 84 | 85 | endmodule 86 | -------------------------------------------------------------------------------- /src/dvp/sim/dummy_xsbl2.v: -------------------------------------------------------------------------------- 1 | 2 | `timescale 1 ns / 1 ps 3 | 4 | module dummy_xsbl2 ( 5 | // Global Control 6 | rst_n, clk, 7 | 8 | // Internal Bus I/F 9 | ibus_cs, 10 | ibus_wr, 11 | ibus_addr_7_2, 12 | ibus_wrdata, 13 | ibus_rddata, 14 | 15 | // Control 16 | rect_done, 17 | xsbl_done, 18 | rect_fcnt, 19 | xsbl_fcnt, 20 | 21 | // DDR Read Arbiter I/F 22 | drd_req, 23 | drd_ack, 24 | drd_vin, 25 | drd_din, 26 | drd_vout, 27 | drd_dout, 28 | 29 | // DDR Write Arbiter I/F 30 | dwr_req, 31 | dwr_ack, 32 | dwr_vout, 33 | dwr_dout, 34 | dwr_strb 35 | ); 36 | 37 | 38 | //========================================================================== 39 | // Port Declaration 40 | //========================================================================== 41 | // Global Control 42 | input rst_n, clk; 43 | 44 | // Internal Bus I/F 45 | input ibus_cs; 46 | input ibus_wr; 47 | input [5:0] ibus_addr_7_2; 48 | input [31:0] ibus_wrdata; 49 | output [31:0] ibus_rddata; 50 | 51 | // Control 52 | input rect_done; 53 | input xsbl_done; 54 | input [3:0] rect_fcnt; 55 | output [3:0] xsbl_fcnt; 56 | 57 | // DDR Read Arbiter I/F 58 | output drd_req; 59 | input drd_ack; 60 | input drd_vin; 61 | input [31:0] drd_din; 62 | output drd_vout; 63 | output [31:0] drd_dout; 64 | 65 | // DDR Write Arbiter I/F 66 | output dwr_req; 67 | input dwr_ack; 68 | output dwr_vout; 69 | output [31:0] dwr_dout; 70 | output [3:0] dwr_strb; 71 | 72 | 73 | //========================================================================== 74 | // Reg/Wire Declaration 75 | //========================================================================== 76 | reg xsbl_sim_start; 77 | reg [9:0] drd_base_a, drd_base_b; 78 | reg [9:0] dwr_base_a, dwr_base_b; 79 | 80 | 81 | //========================================================================== 82 | // Dummy Output 83 | //========================================================================== 84 | assign ibus_rddata[31:0] = 32'b0; 85 | assign drd_req = 1'b0; 86 | assign drd_vout = 1'b0; 87 | assign drd_dout = 32'b0; 88 | assign dwr_req = 1'b0; 89 | assign dwr_vout = 1'b0; 90 | assign dwr_dout = 32'b0; 91 | assign dwr_strb = 4'hF; 92 | 93 | 94 | endmodule 95 | -------------------------------------------------------------------------------- /src/dvp/sim/img_001_l.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/src/dvp/sim/img_001_l.png -------------------------------------------------------------------------------- /src/dvp/sim/img_001_r.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/src/dvp/sim/img_001_r.png -------------------------------------------------------------------------------- /src/dvp/sim/sim_dvp_csi.wcfg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | clk 25 | clk 26 | 27 | 28 | en 29 | en 30 | 31 | 32 | hcnt[31:0] 33 | hcnt[31:0] 34 | 35 | 36 | vcnt[31:0] 37 | vcnt[31:0] 38 | 39 | 40 | addr[31:0] 41 | addr[31:0] 42 | 43 | 44 | valid 45 | valid 46 | 47 | 48 | sof 49 | sof 50 | 51 | 52 | data[15:0] 53 | data[15:0] 54 | HEXRADIX 55 | 56 | 57 | -------------------------------------------------------------------------------- /src/dvp/sim/sim_gpio.wcfg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | gpio_in[1:0] 25 | gpio_in[1:0] 26 | 27 | 28 | gpio_in_r[1:0] 29 | gpio_in_r[1:0] 30 | 31 | 32 | gpio_pos[1:0] 33 | gpio_pos[1:0] 34 | 35 | 36 | gpio_neg[1:0] 37 | gpio_neg[1:0] 38 | 39 | 40 | slv_reg_wren 41 | slv_reg_wren 42 | 43 | 44 | axi_awaddr_r[13:0] 45 | axi_awaddr_r[13:0] 46 | 47 | 48 | axi_wdata[31:0] 49 | axi_wdata[31:0] 50 | 51 | 52 | -------------------------------------------------------------------------------- /src/dvp/sim/sim_sqrt.v: -------------------------------------------------------------------------------- 1 | `timescale 1ns / 1ps 2 | 3 | module sim_sqrt; 4 | 5 | 6 | //========================================================================== 7 | // Reg/Wire Declaration 8 | //========================================================================== 9 | // Reset/Clock Generation 10 | reg clk, rst_n; 11 | 12 | // sqrt 13 | reg [15:0] din; 14 | reg vin; 15 | wire [15:0] dout; 16 | wire vout; 17 | 18 | 19 | //========================================================================== 20 | // Reset/Clock Generation 21 | //========================================================================== 22 | initial begin 23 | clk <= 1'b0; 24 | forever begin 25 | #5; // 100MHz 26 | clk <= ~clk; 27 | end 28 | end 29 | 30 | initial begin 31 | rst_n <= 1'b0; 32 | repeat (5) @(posedge clk); 33 | rst_n <= 1'b1; 34 | end 35 | 36 | 37 | //========================================================================== 38 | // Target Module 39 | //========================================================================== 40 | sqrt sqrt ( 41 | .aclk (clk), 42 | .s_axis_cartesian_tdata (din[15:0]), 43 | .s_axis_cartesian_tvalid (vin), 44 | .m_axis_dout_tdata (dout[15:0]), 45 | .m_axis_dout_tvalid (vout) 46 | ); 47 | 48 | 49 | //========================================================================== 50 | // Simulation Control 51 | //========================================================================== 52 | task INIT; 53 | begin 54 | din <= 16'b0; 55 | vin <= 1'b0; 56 | end 57 | endtask 58 | 59 | initial begin 60 | INIT; 61 | 62 | repeat (10) @(posedge clk); 63 | 64 | din <= 16'd144; 65 | vin <= 1'b1; 66 | @(posedge clk); 67 | din <= 16'd169; 68 | vin <= 1'b1; 69 | @(posedge clk); 70 | vin <= 1'b0; 71 | 72 | repeat (100) @(posedge clk); 73 | 74 | $finish; 75 | end 76 | 77 | 78 | endmodule 79 | -------------------------------------------------------------------------------- /src/dvp/sim/sim_sqrt.wcfg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | clk 25 | clk 26 | 27 | 28 | rst_n 29 | rst_n 30 | 31 | 32 | din[15:0] 33 | din[15:0] 34 | UNSIGNEDDECRADIX 35 | 36 | 37 | vin 38 | vin 39 | 40 | 41 | dout[15:0] 42 | dout[15:0] 43 | UNSIGNEDDECRADIX 44 | 45 | 46 | vout 47 | vout 48 | 49 | 50 | -------------------------------------------------------------------------------- /src/ip_repo/memo.txt: -------------------------------------------------------------------------------- 1 | IP repository directory for Vivado. 2 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/Cholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLESKY_MODULE_H 9 | #define EIGEN_CHOLESKY_MODULE_H 10 | 11 | #include "Core" 12 | #include "Jacobi" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** \defgroup Cholesky_Module Cholesky module 17 | * 18 | * 19 | * 20 | * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. 21 | * Those decompositions are also accessible via the following methods: 22 | * - MatrixBase::llt() 23 | * - MatrixBase::ldlt() 24 | * - SelfAdjointView::llt() 25 | * - SelfAdjointView::ldlt() 26 | * 27 | * \code 28 | * #include 29 | * \endcode 30 | */ 31 | 32 | #include "src/Cholesky/LLT.h" 33 | #include "src/Cholesky/LDLT.h" 34 | #ifdef EIGEN_USE_LAPACKE 35 | #ifdef EIGEN_USE_MKL 36 | #include "mkl_lapacke.h" 37 | #else 38 | #include "src/misc/lapacke.h" 39 | #endif 40 | #include "src/Cholesky/LLT_LAPACKE.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_CHOLESKY_MODULE_H 46 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/CholmodSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLMODSUPPORT_MODULE_H 9 | #define EIGEN_CHOLMODSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup CholmodSupport_Module CholmodSupport module 21 | * 22 | * This module provides an interface to the Cholmod library which is part of the suitesparse package. 23 | * It provides the two following main factorization classes: 24 | * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization. 25 | * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial). 26 | * 27 | * For the sake of completeness, this module also propose the two following classes: 28 | * - class CholmodSimplicialLLT 29 | * - class CholmodSimplicialLDLT 30 | * Note that these classes does not bring any particular advantage compared to the built-in 31 | * SimplicialLLT and SimplicialLDLT factorization classes. 32 | * 33 | * \code 34 | * #include 35 | * \endcode 36 | * 37 | * 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. 38 | * The dependencies depend on how cholmod has been compiled. 39 | * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task. 40 | * 41 | */ 42 | 43 | #include "src/CholmodSupport/CholmodSupport.h" 44 | 45 | #include "src/Core/util/ReenableStupidWarnings.h" 46 | 47 | #endif // EIGEN_CHOLMODSUPPORT_MODULE_H 48 | 49 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/Dense: -------------------------------------------------------------------------------- 1 | #include "Core" 2 | #include "LU" 3 | #include "Cholesky" 4 | #include "QR" 5 | #include "SVD" 6 | #include "Geometry" 7 | #include "Eigenvalues" 8 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/Eigen: -------------------------------------------------------------------------------- 1 | #include "Dense" 2 | #include "Sparse" 3 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/Eigenvalues: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_EIGENVALUES_MODULE_H 9 | #define EIGEN_EIGENVALUES_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "Cholesky" 14 | #include "Jacobi" 15 | #include "Householder" 16 | #include "LU" 17 | #include "Geometry" 18 | 19 | #include "src/Core/util/DisableStupidWarnings.h" 20 | 21 | /** \defgroup Eigenvalues_Module Eigenvalues module 22 | * 23 | * 24 | * 25 | * This module mainly provides various eigenvalue solvers. 26 | * This module also provides some MatrixBase methods, including: 27 | * - MatrixBase::eigenvalues(), 28 | * - MatrixBase::operatorNorm() 29 | * 30 | * \code 31 | * #include 32 | * \endcode 33 | */ 34 | 35 | #include "src/misc/RealSvd2x2.h" 36 | #include "src/Eigenvalues/Tridiagonalization.h" 37 | #include "src/Eigenvalues/RealSchur.h" 38 | #include "src/Eigenvalues/EigenSolver.h" 39 | #include "src/Eigenvalues/SelfAdjointEigenSolver.h" 40 | #include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" 41 | #include "src/Eigenvalues/HessenbergDecomposition.h" 42 | #include "src/Eigenvalues/ComplexSchur.h" 43 | #include "src/Eigenvalues/ComplexEigenSolver.h" 44 | #include "src/Eigenvalues/RealQZ.h" 45 | #include "src/Eigenvalues/GeneralizedEigenSolver.h" 46 | #include "src/Eigenvalues/MatrixBaseEigenvalues.h" 47 | #ifdef EIGEN_USE_LAPACKE 48 | #ifdef EIGEN_USE_MKL 49 | #include "mkl_lapacke.h" 50 | #else 51 | #include "src/misc/lapacke.h" 52 | #endif 53 | #include "src/Eigenvalues/RealSchur_LAPACKE.h" 54 | #include "src/Eigenvalues/ComplexSchur_LAPACKE.h" 55 | #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" 56 | #endif 57 | 58 | #include "src/Core/util/ReenableStupidWarnings.h" 59 | 60 | #endif // EIGEN_EIGENVALUES_MODULE_H 61 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/Geometry: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_GEOMETRY_MODULE_H 9 | #define EIGEN_GEOMETRY_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "SVD" 14 | #include "LU" 15 | #include 16 | 17 | #include "src/Core/util/DisableStupidWarnings.h" 18 | 19 | /** \defgroup Geometry_Module Geometry module 20 | * 21 | * This module provides support for: 22 | * - fixed-size homogeneous transformations 23 | * - translation, scaling, 2D and 3D rotations 24 | * - \link Quaternion quaternions \endlink 25 | * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) 26 | * - orthognal vector generation (\ref MatrixBase::unitOrthogonal) 27 | * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink 28 | * - \link AlignedBox axis aligned bounding boxes \endlink 29 | * - \link umeyama least-square transformation fitting \endlink 30 | * 31 | * \code 32 | * #include 33 | * \endcode 34 | */ 35 | 36 | #include "src/Geometry/OrthoMethods.h" 37 | #include "src/Geometry/EulerAngles.h" 38 | 39 | #include "src/Geometry/Homogeneous.h" 40 | #include "src/Geometry/RotationBase.h" 41 | #include "src/Geometry/Rotation2D.h" 42 | #include "src/Geometry/Quaternion.h" 43 | #include "src/Geometry/AngleAxis.h" 44 | #include "src/Geometry/Transform.h" 45 | #include "src/Geometry/Translation.h" 46 | #include "src/Geometry/Scaling.h" 47 | #include "src/Geometry/Hyperplane.h" 48 | #include "src/Geometry/ParametrizedLine.h" 49 | #include "src/Geometry/AlignedBox.h" 50 | #include "src/Geometry/Umeyama.h" 51 | 52 | // Use the SSE optimized version whenever possible. 53 | #if (defined EIGEN_VECTORIZE_SSE) || (defined EIGEN_VECTORIZE_NEON) 54 | #include "src/Geometry/arch/Geometry_SIMD.h" 55 | #endif 56 | 57 | #include "src/Core/util/ReenableStupidWarnings.h" 58 | 59 | #endif // EIGEN_GEOMETRY_MODULE_H 60 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/Householder: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_HOUSEHOLDER_MODULE_H 9 | #define EIGEN_HOUSEHOLDER_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Householder_Module Householder module 16 | * This module provides Householder transformations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | */ 22 | 23 | #include "src/Householder/Householder.h" 24 | #include "src/Householder/HouseholderSequence.h" 25 | #include "src/Householder/BlockHouseholder.h" 26 | 27 | #include "src/Core/util/ReenableStupidWarnings.h" 28 | 29 | #endif // EIGEN_HOUSEHOLDER_MODULE_H 30 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/IterativeLinearSolvers: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 9 | #define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** 17 | * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module 18 | * 19 | * 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. 20 | * Those solvers are accessible via the following classes: 21 | * - ConjugateGradient for selfadjoint (hermitian) matrices, 22 | * - LeastSquaresConjugateGradient for rectangular least-square problems, 23 | * - BiCGSTAB for general square matrices. 24 | * 25 | * These iterative solvers are associated with some preconditioners: 26 | * - IdentityPreconditioner - not really useful 27 | * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices. 28 | * - IncompleteLUT - incomplete LU factorization with dual thresholding 29 | * 30 | * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. 31 | * 32 | \code 33 | #include 34 | \endcode 35 | */ 36 | 37 | #include "src/IterativeLinearSolvers/SolveWithGuess.h" 38 | #include "src/IterativeLinearSolvers/IterativeSolverBase.h" 39 | #include "src/IterativeLinearSolvers/BasicPreconditioners.h" 40 | #include "src/IterativeLinearSolvers/ConjugateGradient.h" 41 | #include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h" 42 | #include "src/IterativeLinearSolvers/BiCGSTAB.h" 43 | #include "src/IterativeLinearSolvers/IncompleteLUT.h" 44 | #include "src/IterativeLinearSolvers/IncompleteCholesky.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 49 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/Jacobi: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_JACOBI_MODULE_H 9 | #define EIGEN_JACOBI_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Jacobi_Module Jacobi module 16 | * This module provides Jacobi and Givens rotations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | * 22 | * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation: 23 | * - MatrixBase::applyOnTheLeft() 24 | * - MatrixBase::applyOnTheRight(). 25 | */ 26 | 27 | #include "src/Jacobi/Jacobi.h" 28 | 29 | #include "src/Core/util/ReenableStupidWarnings.h" 30 | 31 | #endif // EIGEN_JACOBI_MODULE_H 32 | 33 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/KLUSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_KLUSUPPORT_MODULE_H 9 | #define EIGEN_KLUSUPPORT_MODULE_H 10 | 11 | #include 12 | 13 | #include 14 | 15 | extern "C" { 16 | #include 17 | #include 18 | } 19 | 20 | /** \ingroup Support_modules 21 | * \defgroup KLUSupport_Module KLUSupport module 22 | * 23 | * This module provides an interface to the KLU library which is part of the suitesparse package. 24 | * It provides the following factorization class: 25 | * - class KLU: a sparse LU factorization, well-suited for circuit simulation. 26 | * 27 | * \code 28 | * #include 29 | * \endcode 30 | * 31 | * In order to use this module, the klu and btf headers must be accessible from the include paths, and your binary must be linked to the klu library and its dependencies. 32 | * The dependencies depend on how umfpack has been compiled. 33 | * For a cmake based project, you can use our FindKLU.cmake module to help you in this task. 34 | * 35 | */ 36 | 37 | #include "src/KLUSupport/KLUSupport.h" 38 | 39 | #include 40 | 41 | #endif // EIGEN_KLUSUPPORT_MODULE_H 42 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/LU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_LU_MODULE_H 9 | #define EIGEN_LU_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup LU_Module LU module 16 | * This module includes %LU decomposition and related notions such as matrix inversion and determinant. 17 | * This module defines the following MatrixBase methods: 18 | * - MatrixBase::inverse() 19 | * - MatrixBase::determinant() 20 | * 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | 26 | #include "src/misc/Kernel.h" 27 | #include "src/misc/Image.h" 28 | #include "src/LU/FullPivLU.h" 29 | #include "src/LU/PartialPivLU.h" 30 | #ifdef EIGEN_USE_LAPACKE 31 | #ifdef EIGEN_USE_MKL 32 | #include "mkl_lapacke.h" 33 | #else 34 | #include "src/misc/lapacke.h" 35 | #endif 36 | #include "src/LU/PartialPivLU_LAPACKE.h" 37 | #endif 38 | #include "src/LU/Determinant.h" 39 | #include "src/LU/InverseImpl.h" 40 | 41 | #if defined EIGEN_VECTORIZE_SSE || defined EIGEN_VECTORIZE_NEON 42 | #include "src/LU/arch/InverseSize4.h" 43 | #endif 44 | 45 | #include "src/Core/util/ReenableStupidWarnings.h" 46 | 47 | #endif // EIGEN_LU_MODULE_H 48 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/MetisSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_METISSUPPORT_MODULE_H 9 | #define EIGEN_METISSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | 20 | /** \ingroup Support_modules 21 | * \defgroup MetisSupport_Module MetisSupport module 22 | * 23 | * \code 24 | * #include 25 | * \endcode 26 | * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). 27 | * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink 28 | */ 29 | 30 | 31 | #include "src/MetisSupport/MetisSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_METISSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/OrderingMethods: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ORDERINGMETHODS_MODULE_H 9 | #define EIGEN_ORDERINGMETHODS_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** 16 | * \defgroup OrderingMethods_Module OrderingMethods module 17 | * 18 | * This module is currently for internal use only 19 | * 20 | * It defines various built-in and external ordering methods for sparse matrices. 21 | * They are typically used to reduce the number of elements during 22 | * the sparse matrix decomposition (LLT, LU, QR). 23 | * Precisely, in a preprocessing step, a permutation matrix P is computed using 24 | * those ordering methods and applied to the columns of the matrix. 25 | * Using for instance the sparse Cholesky decomposition, it is expected that 26 | * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A). 27 | * 28 | * 29 | * Usage : 30 | * \code 31 | * #include 32 | * \endcode 33 | * 34 | * A simple usage is as a template parameter in the sparse decomposition classes : 35 | * 36 | * \code 37 | * SparseLU > solver; 38 | * \endcode 39 | * 40 | * \code 41 | * SparseQR > solver; 42 | * \endcode 43 | * 44 | * It is possible as well to call directly a particular ordering method for your own purpose, 45 | * \code 46 | * AMDOrdering ordering; 47 | * PermutationMatrix perm; 48 | * SparseMatrix A; 49 | * //Fill the matrix ... 50 | * 51 | * ordering(A, perm); // Call AMD 52 | * \endcode 53 | * 54 | * \note Some of these methods (like AMD or METIS), need the sparsity pattern 55 | * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, 56 | * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method. 57 | * If your matrix is already symmetric (at leat in structure), you can avoid that 58 | * by calling the method with a SelfAdjointView type. 59 | * 60 | * \code 61 | * // Call the ordering on the pattern of the lower triangular matrix A 62 | * ordering(A.selfadjointView(), perm); 63 | * \endcode 64 | */ 65 | 66 | #include "src/OrderingMethods/Amd.h" 67 | #include "src/OrderingMethods/Ordering.h" 68 | #include "src/Core/util/ReenableStupidWarnings.h" 69 | 70 | #endif // EIGEN_ORDERINGMETHODS_MODULE_H 71 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/PaStiXSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PASTIXSUPPORT_MODULE_H 9 | #define EIGEN_PASTIXSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | #include 18 | } 19 | 20 | #ifdef complex 21 | #undef complex 22 | #endif 23 | 24 | /** \ingroup Support_modules 25 | * \defgroup PaStiXSupport_Module PaStiXSupport module 26 | * 27 | * This module provides an interface to the PaSTiX library. 28 | * PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver. 29 | * It provides the two following main factorization classes: 30 | * - class PastixLLT : a supernodal, parallel LLt Cholesky factorization. 31 | * - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization. 32 | * - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern). 33 | * 34 | * \code 35 | * #include 36 | * \endcode 37 | * 38 | * 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. 39 | * This wrapper resuires PaStiX version 5.x compiled without MPI support. 40 | * The dependencies depend on how PaSTiX has been compiled. 41 | * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task. 42 | * 43 | */ 44 | 45 | #include "src/PaStiXSupport/PaStiXSupport.h" 46 | 47 | #include "src/Core/util/ReenableStupidWarnings.h" 48 | 49 | #endif // EIGEN_PASTIXSUPPORT_MODULE_H 50 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/PardisoSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PARDISOSUPPORT_MODULE_H 9 | #define EIGEN_PARDISOSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup PardisoSupport_Module PardisoSupport module 19 | * 20 | * This module brings support for the Intel(R) MKL PARDISO direct sparse solvers. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * 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. 27 | * See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration. 28 | * 29 | */ 30 | 31 | #include "src/PardisoSupport/PardisoSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_PARDISOSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/QR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QR_MODULE_H 9 | #define EIGEN_QR_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "Cholesky" 14 | #include "Jacobi" 15 | #include "Householder" 16 | 17 | #include "src/Core/util/DisableStupidWarnings.h" 18 | 19 | /** \defgroup QR_Module QR module 20 | * 21 | * 22 | * 23 | * This module provides various QR decompositions 24 | * This module also provides some MatrixBase methods, including: 25 | * - MatrixBase::householderQr() 26 | * - MatrixBase::colPivHouseholderQr() 27 | * - MatrixBase::fullPivHouseholderQr() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/QR/HouseholderQR.h" 35 | #include "src/QR/FullPivHouseholderQR.h" 36 | #include "src/QR/ColPivHouseholderQR.h" 37 | #include "src/QR/CompleteOrthogonalDecomposition.h" 38 | #ifdef EIGEN_USE_LAPACKE 39 | #ifdef EIGEN_USE_MKL 40 | #include "mkl_lapacke.h" 41 | #else 42 | #include "src/misc/lapacke.h" 43 | #endif 44 | #include "src/QR/HouseholderQR_LAPACKE.h" 45 | #include "src/QR/ColPivHouseholderQR_LAPACKE.h" 46 | #endif 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_QR_MODULE_H 51 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/QtAlignedMalloc: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QTMALLOC_MODULE_H 9 | #define EIGEN_QTMALLOC_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #if (!EIGEN_MALLOC_ALREADY_ALIGNED) 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | void *qMalloc(std::size_t size) 18 | { 19 | return Eigen::internal::aligned_malloc(size); 20 | } 21 | 22 | void qFree(void *ptr) 23 | { 24 | Eigen::internal::aligned_free(ptr); 25 | } 26 | 27 | void *qRealloc(void *ptr, std::size_t size) 28 | { 29 | void* newPtr = Eigen::internal::aligned_malloc(size); 30 | std::memcpy(newPtr, ptr, size); 31 | Eigen::internal::aligned_free(ptr); 32 | return newPtr; 33 | } 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | 39 | #endif // EIGEN_QTMALLOC_MODULE_H 40 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/SPQRSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPQRSUPPORT_MODULE_H 9 | #define EIGEN_SPQRSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SuiteSparseQR.hpp" 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup SPQRSupport_Module SuiteSparseQR module 19 | * 20 | * This module provides an interface to the SPQR library, which is part of the suitesparse package. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * 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,...). 27 | * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules 28 | * 29 | */ 30 | 31 | #include "src/CholmodSupport/CholmodSupport.h" 32 | #include "src/SPQRSupport/SuiteSparseQRSupport.h" 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/SVD: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SVD_MODULE_H 9 | #define EIGEN_SVD_MODULE_H 10 | 11 | #include "QR" 12 | #include "Householder" 13 | #include "Jacobi" 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | /** \defgroup SVD_Module SVD module 18 | * 19 | * 20 | * 21 | * This module provides SVD decomposition for matrices (both real and complex). 22 | * Two decomposition algorithms are provided: 23 | * - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very slow for larger ones. 24 | * - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast for large problems. 25 | * These decompositions are accessible via the respective classes and following MatrixBase methods: 26 | * - MatrixBase::jacobiSvd() 27 | * - MatrixBase::bdcSvd() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/misc/RealSvd2x2.h" 35 | #include "src/SVD/UpperBidiagonalization.h" 36 | #include "src/SVD/SVDBase.h" 37 | #include "src/SVD/JacobiSVD.h" 38 | #include "src/SVD/BDCSVD.h" 39 | #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT) 40 | #ifdef EIGEN_USE_MKL 41 | #include "mkl_lapacke.h" 42 | #else 43 | #include "src/misc/lapacke.h" 44 | #endif 45 | #include "src/SVD/JacobiSVD_LAPACKE.h" 46 | #endif 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_SVD_MODULE_H 51 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/Sparse: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSE_MODULE_H 9 | #define EIGEN_SPARSE_MODULE_H 10 | 11 | /** \defgroup Sparse_Module Sparse meta-module 12 | * 13 | * Meta-module including all related modules: 14 | * - \ref SparseCore_Module 15 | * - \ref OrderingMethods_Module 16 | * - \ref SparseCholesky_Module 17 | * - \ref SparseLU_Module 18 | * - \ref SparseQR_Module 19 | * - \ref IterativeLinearSolvers_Module 20 | * 21 | \code 22 | #include 23 | \endcode 24 | */ 25 | 26 | #include "SparseCore" 27 | #include "OrderingMethods" 28 | #include "SparseCholesky" 29 | #include "SparseLU" 30 | #include "SparseQR" 31 | #include "IterativeLinearSolvers" 32 | 33 | #endif // EIGEN_SPARSE_MODULE_H 34 | 35 | -------------------------------------------------------------------------------- /src/slam/include/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 | #include "src/SparseCholesky/SimplicialCholesky.h" 34 | #include "src/SparseCholesky/SimplicialCholesky_impl.h" 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif // EIGEN_SPARSECHOLESKY_MODULE_H 38 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/SparseCore: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSECORE_MODULE_H 9 | #define EIGEN_SPARSECORE_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | /** 22 | * \defgroup SparseCore_Module SparseCore module 23 | * 24 | * This module provides a sparse matrix representation, and basic associated matrix manipulations 25 | * and operations. 26 | * 27 | * See the \ref TutorialSparse "Sparse tutorial" 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | * 33 | * This module depends on: Core. 34 | */ 35 | 36 | #include "src/SparseCore/SparseUtil.h" 37 | #include "src/SparseCore/SparseMatrixBase.h" 38 | #include "src/SparseCore/SparseAssign.h" 39 | #include "src/SparseCore/CompressedStorage.h" 40 | #include "src/SparseCore/AmbiVector.h" 41 | #include "src/SparseCore/SparseCompressedBase.h" 42 | #include "src/SparseCore/SparseMatrix.h" 43 | #include "src/SparseCore/SparseMap.h" 44 | #include "src/SparseCore/MappedSparseMatrix.h" 45 | #include "src/SparseCore/SparseVector.h" 46 | #include "src/SparseCore/SparseRef.h" 47 | #include "src/SparseCore/SparseCwiseUnaryOp.h" 48 | #include "src/SparseCore/SparseCwiseBinaryOp.h" 49 | #include "src/SparseCore/SparseTranspose.h" 50 | #include "src/SparseCore/SparseBlock.h" 51 | #include "src/SparseCore/SparseDot.h" 52 | #include "src/SparseCore/SparseRedux.h" 53 | #include "src/SparseCore/SparseView.h" 54 | #include "src/SparseCore/SparseDiagonalProduct.h" 55 | #include "src/SparseCore/ConservativeSparseSparseProduct.h" 56 | #include "src/SparseCore/SparseSparseProductWithPruning.h" 57 | #include "src/SparseCore/SparseProduct.h" 58 | #include "src/SparseCore/SparseDenseProduct.h" 59 | #include "src/SparseCore/SparseSelfAdjointView.h" 60 | #include "src/SparseCore/SparseTriangularView.h" 61 | #include "src/SparseCore/TriangularSolver.h" 62 | #include "src/SparseCore/SparsePermutation.h" 63 | #include "src/SparseCore/SparseFuzzy.h" 64 | #include "src/SparseCore/SparseSolverBase.h" 65 | 66 | #include "src/Core/util/ReenableStupidWarnings.h" 67 | 68 | #endif // EIGEN_SPARSECORE_MODULE_H 69 | 70 | -------------------------------------------------------------------------------- /src/slam/include/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 | // Ordering interface 24 | #include "OrderingMethods" 25 | 26 | #include "src/Core/util/DisableStupidWarnings.h" 27 | 28 | #include "src/SparseLU/SparseLU_gemm_kernel.h" 29 | 30 | #include "src/SparseLU/SparseLU_Structs.h" 31 | #include "src/SparseLU/SparseLU_SupernodalMatrix.h" 32 | #include "src/SparseLU/SparseLUImpl.h" 33 | #include "src/SparseCore/SparseColEtree.h" 34 | #include "src/SparseLU/SparseLU_Memory.h" 35 | #include "src/SparseLU/SparseLU_heap_relax_snode.h" 36 | #include "src/SparseLU/SparseLU_relax_snode.h" 37 | #include "src/SparseLU/SparseLU_pivotL.h" 38 | #include "src/SparseLU/SparseLU_panel_dfs.h" 39 | #include "src/SparseLU/SparseLU_kernel_bmod.h" 40 | #include "src/SparseLU/SparseLU_panel_bmod.h" 41 | #include "src/SparseLU/SparseLU_column_dfs.h" 42 | #include "src/SparseLU/SparseLU_column_bmod.h" 43 | #include "src/SparseLU/SparseLU_copy_to_ucol.h" 44 | #include "src/SparseLU/SparseLU_pruneL.h" 45 | #include "src/SparseLU/SparseLU_Utils.h" 46 | #include "src/SparseLU/SparseLU.h" 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_SPARSELU_MODULE_H 51 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/SparseQR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSEQR_MODULE_H 9 | #define EIGEN_SPARSEQR_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup SparseQR_Module SparseQR module 16 | * \brief Provides QR decomposition for sparse matrices 17 | * 18 | * This module provides a simplicial version of the left-looking Sparse QR decomposition. 19 | * The columns of the input matrix should be reordered to limit the fill-in during the 20 | * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. 21 | * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 22 | * of built-in and external ordering methods. 23 | * 24 | * \code 25 | * #include 26 | * \endcode 27 | * 28 | * 29 | */ 30 | 31 | #include "src/SparseCore/SparseColEtree.h" 32 | #include "src/SparseQR/SparseQR.h" 33 | 34 | #include "src/Core/util/ReenableStupidWarnings.h" 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /src/slam/include/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 EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes 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 | -------------------------------------------------------------------------------- /src/slam/include/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 EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes 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 | -------------------------------------------------------------------------------- /src/slam/include/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 EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes 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 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/SuperLUSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SUPERLUSUPPORT_MODULE_H 9 | #define EIGEN_SUPERLUSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #ifdef EMPTY 16 | #define EIGEN_EMPTY_WAS_ALREADY_DEFINED 17 | #endif 18 | 19 | typedef int int_t; 20 | #include 21 | #include 22 | #include 23 | 24 | // slu_util.h defines a preprocessor token named EMPTY which is really polluting, 25 | // so we remove it in favor of a SUPERLU_EMPTY token. 26 | // If EMPTY was already defined then we don't undef it. 27 | 28 | #if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED) 29 | # undef EIGEN_EMPTY_WAS_ALREADY_DEFINED 30 | #elif defined(EMPTY) 31 | # undef EMPTY 32 | #endif 33 | 34 | #define SUPERLU_EMPTY (-1) 35 | 36 | namespace Eigen { struct SluMatrix; } 37 | 38 | /** \ingroup Support_modules 39 | * \defgroup SuperLUSupport_Module SuperLUSupport module 40 | * 41 | * This module provides an interface to the SuperLU library. 42 | * It provides the following factorization class: 43 | * - class SuperLU: a supernodal sequential LU factorization. 44 | * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods). 45 | * 46 | * \warning This wrapper requires at least versions 4.0 of SuperLU. The 3.x versions are not supported. 47 | * 48 | * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting. 49 | * 50 | * \code 51 | * #include 52 | * \endcode 53 | * 54 | * 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. 55 | * The dependencies depend on how superlu has been compiled. 56 | * For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task. 57 | * 58 | */ 59 | 60 | #include "src/SuperLUSupport/SuperLUSupport.h" 61 | 62 | #include "src/Core/util/ReenableStupidWarnings.h" 63 | 64 | #endif // EIGEN_SUPERLUSUPPORT_MODULE_H 65 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/UmfPackSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_UMFPACKSUPPORT_MODULE_H 9 | #define EIGEN_UMFPACKSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup UmfPackSupport_Module UmfPackSupport module 21 | * 22 | * This module provides an interface to the UmfPack library which is part of the suitesparse package. 23 | * It provides the following factorization class: 24 | * - class UmfPackLU: a multifrontal sequential LU factorization. 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | * 30 | * 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. 31 | * The dependencies depend on how umfpack has been compiled. 32 | * For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task. 33 | * 34 | */ 35 | 36 | #include "src/UmfPackSupport/UmfPackSupport.h" 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_UMFPACKSUPPORT_MODULE_H 41 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/Assign.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2007 Michael Olbrich 5 | // Copyright (C) 2006-2010 Benoit Jacob 6 | // Copyright (C) 2008 Gael Guennebaud 7 | // 8 | // This Source Code Form is subject to the terms of the Mozilla 9 | // Public License v. 2.0. If a copy of the MPL was not distributed 10 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 11 | 12 | #ifndef EIGEN_ASSIGN_H 13 | #define EIGEN_ASSIGN_H 14 | 15 | namespace Eigen { 16 | 17 | template 18 | template 19 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase 20 | ::lazyAssign(const DenseBase& other) 21 | { 22 | enum{ 23 | SameType = internal::is_same::value 24 | }; 25 | 26 | EIGEN_STATIC_ASSERT_LVALUE(Derived) 27 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived) 28 | EIGEN_STATIC_ASSERT(SameType,YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 29 | 30 | eigen_assert(rows() == other.rows() && cols() == other.cols()); 31 | internal::call_assignment_no_alias(derived(),other.derived()); 32 | 33 | return derived(); 34 | } 35 | 36 | template 37 | template 38 | EIGEN_DEVICE_FUNC 39 | EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) 40 | { 41 | internal::call_assignment(derived(), other.derived()); 42 | return derived(); 43 | } 44 | 45 | template 46 | EIGEN_DEVICE_FUNC 47 | EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) 48 | { 49 | internal::call_assignment(derived(), other.derived()); 50 | return derived(); 51 | } 52 | 53 | template 54 | EIGEN_DEVICE_FUNC 55 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const MatrixBase& other) 56 | { 57 | internal::call_assignment(derived(), other.derived()); 58 | return derived(); 59 | } 60 | 61 | template 62 | template 63 | EIGEN_DEVICE_FUNC 64 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const DenseBase& other) 65 | { 66 | internal::call_assignment(derived(), other.derived()); 67 | return derived(); 68 | } 69 | 70 | template 71 | template 72 | EIGEN_DEVICE_FUNC 73 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const EigenBase& other) 74 | { 75 | internal::call_assignment(derived(), other.derived()); 76 | return derived(); 77 | } 78 | 79 | template 80 | template 81 | EIGEN_DEVICE_FUNC 82 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const ReturnByValue& other) 83 | { 84 | other.derived().evalTo(derived()); 85 | return derived(); 86 | } 87 | 88 | } // end namespace Eigen 89 | 90 | #endif // EIGEN_ASSIGN_H 91 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/DiagonalProduct.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) 2007-2009 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 EIGEN_DIAGONALPRODUCT_H 12 | #define EIGEN_DIAGONALPRODUCT_H 13 | 14 | namespace Eigen { 15 | 16 | /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal. 17 | */ 18 | template 19 | template 20 | EIGEN_DEVICE_FUNC inline const Product 21 | MatrixBase::operator*(const DiagonalBase &a_diagonal) const 22 | { 23 | return Product(derived(),a_diagonal.derived()); 24 | } 25 | 26 | } // end namespace Eigen 27 | 28 | #endif // EIGEN_DIAGONALPRODUCT_H 29 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/NestByValue.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) 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 EIGEN_NESTBYVALUE_H 12 | #define EIGEN_NESTBYVALUE_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | template 18 | struct traits > : public traits 19 | { 20 | enum { 21 | Flags = traits::Flags & ~NestByRefBit 22 | }; 23 | }; 24 | } 25 | 26 | /** \class NestByValue 27 | * \ingroup Core_Module 28 | * 29 | * \brief Expression which must be nested by value 30 | * 31 | * \tparam ExpressionType the type of the object of which we are requiring nesting-by-value 32 | * 33 | * This class is the return type of MatrixBase::nestByValue() 34 | * and most of the time this is the only way it is used. 35 | * 36 | * \sa MatrixBase::nestByValue() 37 | */ 38 | template class NestByValue 39 | : public internal::dense_xpr_base< NestByValue >::type 40 | { 41 | public: 42 | 43 | typedef typename internal::dense_xpr_base::type Base; 44 | EIGEN_DENSE_PUBLIC_INTERFACE(NestByValue) 45 | 46 | EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {} 47 | 48 | EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); } 49 | EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); } 50 | 51 | EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; } 52 | 53 | EIGEN_DEVICE_FUNC const ExpressionType& nestedExpression() const { return m_expression; } 54 | 55 | protected: 56 | const ExpressionType m_expression; 57 | }; 58 | 59 | /** \returns an expression of the temporary version of *this. 60 | */ 61 | template 62 | EIGEN_DEVICE_FUNC inline const NestByValue 63 | DenseBase::nestByValue() const 64 | { 65 | return NestByValue(derived()); 66 | } 67 | 68 | namespace internal { 69 | 70 | // Evaluator of Solve -> eval into a temporary 71 | template 72 | struct evaluator > 73 | : public evaluator 74 | { 75 | typedef evaluator Base; 76 | 77 | EIGEN_DEVICE_FUNC explicit evaluator(const NestByValue& xpr) 78 | : Base(xpr.nestedExpression()) 79 | {} 80 | }; 81 | } 82 | 83 | } // end namespace Eigen 84 | 85 | #endif // EIGEN_NESTBYVALUE_H 86 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/SelfCwiseBinaryOp.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-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_SELFCWISEBINARYOP_H 11 | #define EIGEN_SELFCWISEBINARYOP_H 12 | 13 | namespace Eigen { 14 | 15 | // TODO generalize the scalar type of 'other' 16 | 17 | template 18 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator*=(const Scalar& other) 19 | { 20 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::mul_assign_op()); 21 | return derived(); 22 | } 23 | 24 | template 25 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const Scalar& other) 26 | { 27 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::add_assign_op()); 28 | return derived(); 29 | } 30 | 31 | template 32 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const Scalar& other) 33 | { 34 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::sub_assign_op()); 35 | return derived(); 36 | } 37 | 38 | template 39 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator/=(const Scalar& other) 40 | { 41 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::div_assign_op()); 42 | return derived(); 43 | } 44 | 45 | } // end namespace Eigen 46 | 47 | #endif // EIGEN_SELFCWISEBINARYOP_H 48 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/Swap.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2006-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_SWAP_H 11 | #define EIGEN_SWAP_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // Overload default assignPacket behavior for swapping them 18 | template 19 | class generic_dense_assignment_kernel, Specialized> 20 | : public generic_dense_assignment_kernel, BuiltIn> 21 | { 22 | protected: 23 | typedef generic_dense_assignment_kernel, BuiltIn> Base; 24 | using Base::m_dst; 25 | using Base::m_src; 26 | using Base::m_functor; 27 | 28 | public: 29 | typedef typename Base::Scalar Scalar; 30 | typedef typename Base::DstXprType DstXprType; 31 | typedef swap_assign_op Functor; 32 | 33 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 34 | generic_dense_assignment_kernel(DstEvaluatorTypeT &dst, const SrcEvaluatorTypeT &src, const Functor &func, DstXprType& dstExpr) 35 | : Base(dst, src, func, dstExpr) 36 | {} 37 | 38 | template 39 | EIGEN_STRONG_INLINE void assignPacket(Index row, Index col) 40 | { 41 | PacketType tmp = m_src.template packet(row,col); 42 | const_cast(m_src).template writePacket(row,col, m_dst.template packet(row,col)); 43 | m_dst.template writePacket(row,col,tmp); 44 | } 45 | 46 | template 47 | EIGEN_STRONG_INLINE void assignPacket(Index index) 48 | { 49 | PacketType tmp = m_src.template packet(index); 50 | const_cast(m_src).template writePacket(index, m_dst.template packet(index)); 51 | m_dst.template writePacket(index,tmp); 52 | } 53 | 54 | // TODO find a simple way not to have to copy/paste this function from generic_dense_assignment_kernel, by simple I mean no CRTP (Gael) 55 | template 56 | EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner) 57 | { 58 | Index row = Base::rowIndexByOuterInner(outer, inner); 59 | Index col = Base::colIndexByOuterInner(outer, inner); 60 | assignPacket(row, col); 61 | } 62 | }; 63 | 64 | } // namespace internal 65 | 66 | } // end namespace Eigen 67 | 68 | #endif // EIGEN_SWAP_H 69 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/arch/AVX/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 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_TYPE_CASTING_AVX_H 11 | #define EIGEN_TYPE_CASTING_AVX_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // For now we use SSE to handle integers, so we can't use AVX instructions to cast 18 | // from int to float 19 | template <> 20 | struct type_casting_traits { 21 | enum { 22 | VectorizedCast = 0, 23 | SrcCoeffRatio = 1, 24 | TgtCoeffRatio = 1 25 | }; 26 | }; 27 | 28 | template <> 29 | struct type_casting_traits { 30 | enum { 31 | VectorizedCast = 0, 32 | SrcCoeffRatio = 1, 33 | TgtCoeffRatio = 1 34 | }; 35 | }; 36 | 37 | 38 | #ifndef EIGEN_VECTORIZE_AVX512 39 | 40 | template <> 41 | struct type_casting_traits { 42 | enum { 43 | VectorizedCast = 1, 44 | SrcCoeffRatio = 1, 45 | TgtCoeffRatio = 1 46 | }; 47 | }; 48 | 49 | 50 | template <> 51 | struct type_casting_traits { 52 | enum { 53 | VectorizedCast = 1, 54 | SrcCoeffRatio = 1, 55 | TgtCoeffRatio = 1 56 | }; 57 | }; 58 | 59 | template <> 60 | struct type_casting_traits { 61 | enum { 62 | VectorizedCast = 1, 63 | SrcCoeffRatio = 1, 64 | TgtCoeffRatio = 1 65 | }; 66 | }; 67 | 68 | template <> 69 | struct type_casting_traits { 70 | enum { 71 | VectorizedCast = 1, 72 | SrcCoeffRatio = 1, 73 | TgtCoeffRatio = 1 74 | }; 75 | }; 76 | 77 | #endif // EIGEN_VECTORIZE_AVX512 78 | 79 | template<> EIGEN_STRONG_INLINE Packet8i pcast(const Packet8f& a) { 80 | return _mm256_cvttps_epi32(a); 81 | } 82 | 83 | template<> EIGEN_STRONG_INLINE Packet8f pcast(const Packet8i& a) { 84 | return _mm256_cvtepi32_ps(a); 85 | } 86 | 87 | template<> EIGEN_STRONG_INLINE Packet8i preinterpret(const Packet8f& a) { 88 | return _mm256_castps_si256(a); 89 | } 90 | 91 | template<> EIGEN_STRONG_INLINE Packet8f preinterpret(const Packet8i& a) { 92 | return _mm256_castsi256_ps(a); 93 | } 94 | 95 | template<> EIGEN_STRONG_INLINE Packet8f pcast(const Packet8h& a) { 96 | return half2float(a); 97 | } 98 | 99 | template<> EIGEN_STRONG_INLINE Packet8f pcast(const Packet8bf& a) { 100 | return Bf16ToF32(a); 101 | } 102 | 103 | template<> EIGEN_STRONG_INLINE Packet8h pcast(const Packet8f& a) { 104 | return float2half(a); 105 | } 106 | 107 | template<> EIGEN_STRONG_INLINE Packet8bf pcast(const Packet8f& a) { 108 | return F32ToBf16(a); 109 | } 110 | 111 | } // end namespace internal 112 | 113 | } // end namespace Eigen 114 | 115 | #endif // EIGEN_TYPE_CASTING_AVX_H 116 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/arch/AVX512/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2019 Rasmus Munk Larsen 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_TYPE_CASTING_AVX512_H 11 | #define EIGEN_TYPE_CASTING_AVX512_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | template<> EIGEN_STRONG_INLINE Packet16i pcast(const Packet16f& a) { 18 | return _mm512_cvttps_epi32(a); 19 | } 20 | 21 | template<> EIGEN_STRONG_INLINE Packet16f pcast(const Packet16i& a) { 22 | return _mm512_cvtepi32_ps(a); 23 | } 24 | 25 | template<> EIGEN_STRONG_INLINE Packet16i preinterpret(const Packet16f& a) { 26 | return _mm512_castps_si512(a); 27 | } 28 | 29 | template<> EIGEN_STRONG_INLINE Packet16f preinterpret(const Packet16i& a) { 30 | return _mm512_castsi512_ps(a); 31 | } 32 | 33 | template <> 34 | struct type_casting_traits { 35 | enum { 36 | VectorizedCast = 1, 37 | SrcCoeffRatio = 1, 38 | TgtCoeffRatio = 1 39 | }; 40 | }; 41 | 42 | template<> EIGEN_STRONG_INLINE Packet16f pcast(const Packet16h& a) { 43 | return half2float(a); 44 | } 45 | 46 | template <> 47 | struct type_casting_traits { 48 | enum { 49 | VectorizedCast = 1, 50 | SrcCoeffRatio = 1, 51 | TgtCoeffRatio = 1 52 | }; 53 | }; 54 | 55 | template<> EIGEN_STRONG_INLINE Packet16h pcast(const Packet16f& a) { 56 | return float2half(a); 57 | } 58 | 59 | template <> 60 | struct type_casting_traits { 61 | enum { 62 | VectorizedCast = 1, 63 | SrcCoeffRatio = 1, 64 | TgtCoeffRatio = 1 65 | }; 66 | }; 67 | 68 | template<> EIGEN_STRONG_INLINE Packet16f pcast(const Packet16bf& a) { 69 | return Bf16ToF32(a); 70 | } 71 | 72 | template <> 73 | struct type_casting_traits { 74 | enum { 75 | VectorizedCast = 1, 76 | SrcCoeffRatio = 1, 77 | TgtCoeffRatio = 1 78 | }; 79 | }; 80 | 81 | template<> EIGEN_STRONG_INLINE Packet16bf pcast(const Packet16f& a) { 82 | return F32ToBf16(a); 83 | } 84 | 85 | } // end namespace internal 86 | 87 | } // end namespace Eigen 88 | 89 | #endif // EIGEN_TYPE_CASTING_AVX512_H 90 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/arch/AltiVec/MathFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2007 Julien Pommier 5 | // Copyright (C) 2009 Gael Guennebaud 6 | // Copyright (C) 2016 Konstantinos Margaritis 7 | // 8 | // This Source Code Form is subject to the terms of the Mozilla 9 | // Public License v. 2.0. If a copy of the MPL was not distributed 10 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 11 | 12 | #ifndef EIGEN_MATH_FUNCTIONS_ALTIVEC_H 13 | #define EIGEN_MATH_FUNCTIONS_ALTIVEC_H 14 | 15 | namespace Eigen { 16 | 17 | namespace internal { 18 | 19 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 20 | Packet4f plog(const Packet4f& _x) 21 | { 22 | return plog_float(_x); 23 | } 24 | 25 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 26 | Packet4f pexp(const Packet4f& _x) 27 | { 28 | return pexp_float(_x); 29 | } 30 | 31 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 32 | Packet4f psin(const Packet4f& _x) 33 | { 34 | return psin_float(_x); 35 | } 36 | 37 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 38 | Packet4f pcos(const Packet4f& _x) 39 | { 40 | return pcos_float(_x); 41 | } 42 | 43 | #ifndef EIGEN_COMP_CLANG 44 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 45 | Packet4f prsqrt(const Packet4f& x) 46 | { 47 | return vec_rsqrt(x); 48 | } 49 | #endif 50 | 51 | #ifdef __VSX__ 52 | #ifndef EIGEN_COMP_CLANG 53 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 54 | Packet2d prsqrt(const Packet2d& x) 55 | { 56 | return vec_rsqrt(x); 57 | } 58 | #endif 59 | 60 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 61 | Packet4f psqrt(const Packet4f& x) 62 | { 63 | return vec_sqrt(x); 64 | } 65 | 66 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 67 | Packet2d psqrt(const Packet2d& x) 68 | { 69 | return vec_sqrt(x); 70 | } 71 | 72 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 73 | Packet2d pexp(const Packet2d& _x) 74 | { 75 | return pexp_double(_x); 76 | } 77 | #endif 78 | 79 | // Hyperbolic Tangent function. 80 | template <> 81 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f 82 | ptanh(const Packet4f& x) { 83 | return internal::generic_fast_tanh_float(x); 84 | } 85 | 86 | } // end namespace internal 87 | 88 | } // end namespace Eigen 89 | 90 | #endif // EIGEN_MATH_FUNCTIONS_ALTIVEC_H 91 | -------------------------------------------------------------------------------- /src/slam/include/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 110 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 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/arch/GPU/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Benoit Steiner 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_TYPE_CASTING_GPU_H 11 | #define EIGEN_TYPE_CASTING_GPU_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \ 18 | (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) 19 | 20 | 21 | template <> 22 | struct type_casting_traits { 23 | enum { 24 | VectorizedCast = 1, 25 | SrcCoeffRatio = 1, 26 | TgtCoeffRatio = 2 27 | }; 28 | }; 29 | 30 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float4 pcast(const half2& a, const half2& b) { 31 | float2 r1 = __half22float2(a); 32 | float2 r2 = __half22float2(b); 33 | return make_float4(r1.x, r1.y, r2.x, r2.y); 34 | } 35 | 36 | 37 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4h2 pcast(const float4& a, const float4& b) { 38 | Packet4h2 r; 39 | half2* r_alias=reinterpret_cast(&r); 40 | r_alias[0]=__floats2half2_rn(a.x,a.y); 41 | r_alias[1]=__floats2half2_rn(a.z,a.w); 42 | r_alias[2]=__floats2half2_rn(b.x,b.y); 43 | r_alias[3]=__floats2half2_rn(b.z,b.w); 44 | return r; 45 | } 46 | 47 | template <> 48 | struct type_casting_traits { 49 | enum { 50 | VectorizedCast = 1, 51 | SrcCoeffRatio = 2, 52 | TgtCoeffRatio = 1 53 | }; 54 | }; 55 | 56 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float4 pcast(const Packet4h2& a) { 57 | // Simply discard the second half of the input 58 | float4 r; 59 | const half2* a_alias=reinterpret_cast(&a); 60 | float2 r1 = __half22float2(a_alias[0]); 61 | float2 r2 = __half22float2(a_alias[1]); 62 | r.x=static_cast(r1.x); 63 | r.y=static_cast(r1.y); 64 | r.z=static_cast(r2.x); 65 | r.w=static_cast(r2.y); 66 | return r; 67 | } 68 | 69 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE half2 pcast(const float4& a) { 70 | // Simply discard the second half of the input 71 | return __floats2half2_rn(a.x, a.y); 72 | } 73 | 74 | #endif 75 | 76 | } // end namespace internal 77 | 78 | } // end namespace Eigen 79 | 80 | #endif // EIGEN_TYPE_CASTING_GPU_H 81 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/arch/HIP/hcc/math_constants.h: -------------------------------------------------------------------------------- 1 | /* 2 | * math_constants.h - 3 | * HIP equivalent of the CUDA header of the same name 4 | */ 5 | 6 | #ifndef __MATH_CONSTANTS_H__ 7 | #define __MATH_CONSTANTS_H__ 8 | 9 | /* single precision constants */ 10 | 11 | #define HIPRT_INF_F __int_as_float(0x7f800000) 12 | #define HIPRT_NAN_F __int_as_float(0x7fffffff) 13 | #define HIPRT_MIN_DENORM_F __int_as_float(0x00000001) 14 | #define HIPRT_MAX_NORMAL_F __int_as_float(0x7f7fffff) 15 | #define HIPRT_NEG_ZERO_F __int_as_float(0x80000000) 16 | #define HIPRT_ZERO_F 0.0f 17 | #define HIPRT_ONE_F 1.0f 18 | 19 | /* double precision constants */ 20 | #define HIPRT_INF __hiloint2double(0x7ff00000, 0x00000000) 21 | #define HIPRT_NAN __hiloint2double(0xfff80000, 0x00000000) 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/arch/SVE/MathFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2020, Arm Limited and Contributors 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_MATH_FUNCTIONS_SVE_H 11 | #define EIGEN_MATH_FUNCTIONS_SVE_H 12 | 13 | namespace Eigen { 14 | namespace internal { 15 | 16 | template <> 17 | EIGEN_STRONG_INLINE EIGEN_UNUSED PacketXf pexp(const PacketXf& x) { 18 | return pexp_float(x); 19 | } 20 | 21 | template <> 22 | EIGEN_STRONG_INLINE EIGEN_UNUSED PacketXf plog(const PacketXf& x) { 23 | return plog_float(x); 24 | } 25 | 26 | template <> 27 | EIGEN_STRONG_INLINE EIGEN_UNUSED PacketXf psin(const PacketXf& x) { 28 | return psin_float(x); 29 | } 30 | 31 | template <> 32 | EIGEN_STRONG_INLINE EIGEN_UNUSED PacketXf pcos(const PacketXf& x) { 33 | return pcos_float(x); 34 | } 35 | 36 | // Hyperbolic Tangent function. 37 | template <> 38 | EIGEN_STRONG_INLINE EIGEN_UNUSED PacketXf ptanh(const PacketXf& x) { 39 | return internal::generic_fast_tanh_float(x); 40 | } 41 | } // end namespace internal 42 | } // end namespace Eigen 43 | 44 | #endif // EIGEN_MATH_FUNCTIONS_SVE_H 45 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/arch/SVE/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2020, Arm Limited and Contributors 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_TYPE_CASTING_SVE_H 11 | #define EIGEN_TYPE_CASTING_SVE_H 12 | 13 | namespace Eigen { 14 | namespace internal { 15 | 16 | template <> 17 | struct type_casting_traits { 18 | enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 }; 19 | }; 20 | 21 | template <> 22 | struct type_casting_traits { 23 | enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 }; 24 | }; 25 | 26 | template <> 27 | EIGEN_STRONG_INLINE PacketXf pcast(const PacketXi& a) { 28 | return svcvt_f32_s32_z(svptrue_b32(), a); 29 | } 30 | 31 | template <> 32 | EIGEN_STRONG_INLINE PacketXi pcast(const PacketXf& a) { 33 | return svcvt_s32_f32_z(svptrue_b32(), a); 34 | } 35 | 36 | template <> 37 | EIGEN_STRONG_INLINE PacketXf preinterpret(const PacketXi& a) { 38 | return svreinterpret_f32_s32(a); 39 | } 40 | 41 | template <> 42 | EIGEN_STRONG_INLINE PacketXi preinterpret(const PacketXf& a) { 43 | return svreinterpret_s32_f32(a); 44 | } 45 | 46 | } // namespace internal 47 | } // namespace Eigen 48 | 49 | #endif // EIGEN_TYPE_CASTING_SVE_H 50 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/arch/SYCL/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Mehdi Goli Codeplay Software Ltd. 5 | // Ralph Potter Codeplay Software Ltd. 6 | // Luke Iwanski Codeplay Software Ltd. 7 | // Contact: 8 | // 9 | // This Source Code Form is subject to the terms of the Mozilla 10 | // Public License v. 2.0. If a copy of the MPL was not distributed 11 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 12 | 13 | /***************************************************************** 14 | * TypeCasting.h 15 | * 16 | * \brief: 17 | * TypeCasting 18 | * 19 | *****************************************************************/ 20 | 21 | #ifndef EIGEN_TYPE_CASTING_SYCL_H 22 | #define EIGEN_TYPE_CASTING_SYCL_H 23 | 24 | namespace Eigen { 25 | 26 | namespace internal { 27 | #ifdef SYCL_DEVICE_ONLY 28 | template <> 29 | struct type_casting_traits { 30 | enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 }; 31 | }; 32 | 33 | template <> 34 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE cl::sycl::cl_int4 35 | pcast(const cl::sycl::cl_float4& a) { 36 | return a 37 | .template convert(); 38 | } 39 | 40 | template <> 41 | struct type_casting_traits { 42 | enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 }; 43 | }; 44 | 45 | template <> 46 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE cl::sycl::cl_float4 47 | pcast(const cl::sycl::cl_int4& a) { 48 | return a.template convert(); 50 | } 51 | 52 | template <> 53 | struct type_casting_traits { 54 | enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 }; 55 | }; 56 | 57 | template <> 58 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE cl::sycl::cl_float4 59 | pcast( 60 | const cl::sycl::cl_double2& a, const cl::sycl::cl_double2& b) { 61 | auto a1 = a.template convert(); 63 | auto b1 = b.template convert(); 65 | return cl::sycl::float4(a1.x(), a1.y(), b1.x(), b1.y()); 66 | } 67 | 68 | template <> 69 | struct type_casting_traits { 70 | enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 }; 71 | }; 72 | 73 | template <> 74 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE cl::sycl::cl_double2 75 | pcast(const cl::sycl::cl_float4& a) { 76 | // Simply discard the second half of the input 77 | return cl::sycl::cl_double2(a.x(), a.y()); 78 | } 79 | 80 | #endif 81 | } // end namespace internal 82 | 83 | } // end namespace Eigen 84 | 85 | #endif // EIGEN_TYPE_CASTING_SYCL_H 86 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/functors/TernaryFunctors.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Eugene Brevdo 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_TERNARY_FUNCTORS_H 11 | #define EIGEN_TERNARY_FUNCTORS_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | //---------- associative ternary functors ---------- 18 | 19 | 20 | 21 | } // end namespace internal 22 | 23 | } // end namespace Eigen 24 | 25 | #endif // EIGEN_TERNARY_FUNCTORS_H 26 | -------------------------------------------------------------------------------- /src/slam/include/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 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/util/ReenableStupidWarnings.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_WARNINGS_DISABLED_2 2 | // "DisableStupidWarnings.h" was included twice recursively: Do not reenable warnings yet! 3 | # undef EIGEN_WARNINGS_DISABLED_2 4 | 5 | #elif defined(EIGEN_WARNINGS_DISABLED) 6 | #undef EIGEN_WARNINGS_DISABLED 7 | 8 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 9 | #ifdef _MSC_VER 10 | #pragma warning( pop ) 11 | #elif defined __INTEL_COMPILER 12 | #pragma warning pop 13 | #elif defined __clang__ 14 | #pragma clang diagnostic pop 15 | #elif defined __GNUC__ && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) 16 | #pragma GCC diagnostic pop 17 | #endif 18 | 19 | #if defined __NVCC__ 20 | // Don't reenable the diagnostic messages, as it turns out these messages need 21 | // to be disabled at the point of the template instantiation (i.e the user code) 22 | // otherwise they'll be triggered by nvcc. 23 | // #pragma diag_default code_is_unreachable 24 | // #pragma diag_default initialization_not_reachable 25 | // #pragma diag_default 2651 26 | // #pragma diag_default 2653 27 | #endif 28 | 29 | #endif 30 | 31 | #endif // EIGEN_WARNINGS_DISABLED 32 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/Core/util/ReshapedHelper.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2017 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 | 11 | #ifndef EIGEN_RESHAPED_HELPER_H 12 | #define EIGEN_RESHAPED_HELPER_H 13 | 14 | namespace Eigen { 15 | 16 | enum AutoSize_t { AutoSize }; 17 | const int AutoOrder = 2; 18 | 19 | namespace internal { 20 | 21 | template 22 | struct get_compiletime_reshape_size { 23 | enum { value = get_fixed_value::value }; 24 | }; 25 | 26 | template 27 | Index get_runtime_reshape_size(SizeType size, Index /*other*/, Index /*total*/) { 28 | return internal::get_runtime_value(size); 29 | } 30 | 31 | template 32 | struct get_compiletime_reshape_size { 33 | enum { 34 | other_size = get_fixed_value::value, 35 | value = (TotalSize==Dynamic || other_size==Dynamic) ? Dynamic : TotalSize / other_size }; 36 | }; 37 | 38 | inline Index get_runtime_reshape_size(AutoSize_t /*size*/, Index other, Index total) { 39 | return total/other; 40 | } 41 | 42 | template 43 | struct get_compiletime_reshape_order { 44 | enum { value = Order == AutoOrder ? Flags & RowMajorBit : Order }; 45 | }; 46 | 47 | } 48 | 49 | } // end namespace Eigen 50 | 51 | #endif // EIGEN_RESHAPED_HELPER_H 52 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/SparseCore/MappedSparseMatrix.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 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_MAPPED_SPARSEMATRIX_H 11 | #define EIGEN_MAPPED_SPARSEMATRIX_H 12 | 13 | namespace Eigen { 14 | 15 | /** \deprecated Use Map > 16 | * \class MappedSparseMatrix 17 | * 18 | * \brief Sparse matrix 19 | * 20 | * \param _Scalar the scalar type, i.e. the type of the coefficients 21 | * 22 | * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme. 23 | * 24 | */ 25 | namespace internal { 26 | template 27 | struct traits > : traits > 28 | {}; 29 | } // end namespace internal 30 | 31 | template 32 | class MappedSparseMatrix 33 | : public Map > 34 | { 35 | typedef Map > Base; 36 | 37 | public: 38 | 39 | typedef typename Base::StorageIndex StorageIndex; 40 | typedef typename Base::Scalar Scalar; 41 | 42 | inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0) 43 | : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr) 44 | {} 45 | 46 | /** Empty destructor */ 47 | inline ~MappedSparseMatrix() {} 48 | }; 49 | 50 | namespace internal { 51 | 52 | template 53 | struct evaluator > 54 | : evaluator > > 55 | { 56 | typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType; 57 | typedef evaluator > Base; 58 | 59 | evaluator() : Base() {} 60 | explicit evaluator(const XprType &mat) : Base(mat) {} 61 | }; 62 | 63 | } 64 | 65 | } // end namespace Eigen 66 | 67 | #endif // EIGEN_MAPPED_SPARSEMATRIX_H 68 | -------------------------------------------------------------------------------- /src/slam/include/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-2014 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 | namespace Eigen { 14 | 15 | template 16 | template 17 | bool SparseMatrixBase::isApprox(const SparseMatrixBase& other, const RealScalar &prec) const 18 | { 19 | const typename internal::nested_eval::type actualA(derived()); 20 | typename internal::conditional::type, 22 | const PlainObject>::type actualB(other.derived()); 23 | 24 | return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm()); 25 | } 26 | 27 | } // end namespace Eigen 28 | 29 | #endif // EIGEN_SPARSE_FUZZY_H 30 | -------------------------------------------------------------------------------- /src/slam/include/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-2014 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 | internal::evaluator thisEval(derived()); 22 | for (Index j=0; j::InnerIterator iter(thisEval,j); iter; ++iter) 24 | res += iter.value(); 25 | return res; 26 | } 27 | 28 | template 29 | typename internal::traits >::Scalar 30 | SparseMatrix<_Scalar,_Options,_Index>::sum() const 31 | { 32 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 33 | if(this->isCompressed()) 34 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 35 | else 36 | return Base::sum(); 37 | } 38 | 39 | template 40 | typename internal::traits >::Scalar 41 | SparseVector<_Scalar,_Options,_Index>::sum() const 42 | { 43 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 44 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 45 | } 46 | 47 | } // end namespace Eigen 48 | 49 | #endif // EIGEN_SPARSEREDUX_H 50 | -------------------------------------------------------------------------------- /src/slam/include/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 | StorageIndex 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 | -------------------------------------------------------------------------------- /src/slam/include/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 parent; 52 | relax_end.setConstant(emptyIdxLU); 53 | descendants.setZero(); 54 | for (Index 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 (Index 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) = StorageIndex(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 | -------------------------------------------------------------------------------- /src/slam/include/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 std::size_t size_type; 26 | typedef std::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 | #if EIGEN_COMP_MSVC 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 | -------------------------------------------------------------------------------- /src/slam/include/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 | 42 | image_retval_base(const DecompositionType& dec, const MatrixType& originalMatrix) 43 | : m_dec(dec), m_rank(dec.rank()), 44 | m_cols(m_rank == 0 ? 1 : m_rank), 45 | m_originalMatrix(originalMatrix) 46 | {} 47 | 48 | inline Index rows() const { return m_dec.rows(); } 49 | inline Index cols() const { return m_cols; } 50 | inline Index rank() const { return m_rank; } 51 | inline const DecompositionType& dec() const { return m_dec; } 52 | inline const MatrixType& originalMatrix() const { return m_originalMatrix; } 53 | 54 | template inline void evalTo(Dest& dst) const 55 | { 56 | static_cast*>(this)->evalTo(dst); 57 | } 58 | 59 | protected: 60 | const DecompositionType& m_dec; 61 | Index m_rank, m_cols; 62 | const MatrixType& m_originalMatrix; 63 | }; 64 | 65 | } // end namespace internal 66 | 67 | #define EIGEN_MAKE_IMAGE_HELPERS(DecompositionType) \ 68 | typedef typename DecompositionType::MatrixType MatrixType; \ 69 | typedef typename MatrixType::Scalar Scalar; \ 70 | typedef typename MatrixType::RealScalar RealScalar; \ 71 | typedef Eigen::internal::image_retval_base Base; \ 72 | using Base::dec; \ 73 | using Base::originalMatrix; \ 74 | using Base::rank; \ 75 | using Base::rows; \ 76 | using Base::cols; \ 77 | image_retval(const DecompositionType& dec, const MatrixType& originalMatrix) \ 78 | : Base(dec, originalMatrix) {} 79 | 80 | } // end namespace Eigen 81 | 82 | #endif // EIGEN_MISC_IMAGE_H 83 | -------------------------------------------------------------------------------- /src/slam/include/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 | 43 | explicit kernel_retval_base(const DecompositionType& dec) 44 | : m_dec(dec), 45 | m_rank(dec.rank()), 46 | m_cols(m_rank==dec.cols() ? 1 : dec.cols() - m_rank) 47 | {} 48 | 49 | inline Index rows() const { return m_dec.cols(); } 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 | 54 | template inline void evalTo(Dest& dst) const 55 | { 56 | static_cast*>(this)->evalTo(dst); 57 | } 58 | 59 | protected: 60 | const DecompositionType& m_dec; 61 | Index m_rank, m_cols; 62 | }; 63 | 64 | } // end namespace internal 65 | 66 | #define EIGEN_MAKE_KERNEL_HELPERS(DecompositionType) \ 67 | typedef typename DecompositionType::MatrixType MatrixType; \ 68 | typedef typename MatrixType::Scalar Scalar; \ 69 | typedef typename MatrixType::RealScalar RealScalar; \ 70 | typedef Eigen::internal::kernel_retval_base Base; \ 71 | using Base::dec; \ 72 | using Base::rank; \ 73 | using Base::rows; \ 74 | using Base::cols; \ 75 | kernel_retval(const DecompositionType& dec) : Base(dec) {} 76 | 77 | } // end namespace Eigen 78 | 79 | #endif // EIGEN_MISC_KERNEL_H 80 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/misc/RealSvd2x2.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Benoit Jacob 5 | // Copyright (C) 2013-2016 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_REALSVD2X2_H 12 | #define EIGEN_REALSVD2X2_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | template 19 | void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, 20 | JacobiRotation *j_left, 21 | JacobiRotation *j_right) 22 | { 23 | using std::sqrt; 24 | using std::abs; 25 | Matrix m; 26 | m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)), 27 | numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q)); 28 | JacobiRotation rot1; 29 | RealScalar t = m.coeff(0,0) + m.coeff(1,1); 30 | RealScalar d = m.coeff(1,0) - m.coeff(0,1); 31 | 32 | if(abs(d) < (std::numeric_limits::min)()) 33 | { 34 | rot1.s() = RealScalar(0); 35 | rot1.c() = RealScalar(1); 36 | } 37 | else 38 | { 39 | // If d!=0, then t/d cannot overflow because the magnitude of the 40 | // entries forming d are not too small compared to the ones forming t. 41 | RealScalar u = t / d; 42 | RealScalar tmp = sqrt(RealScalar(1) + numext::abs2(u)); 43 | rot1.s() = RealScalar(1) / tmp; 44 | rot1.c() = u / tmp; 45 | } 46 | m.applyOnTheLeft(0,1,rot1); 47 | j_right->makeJacobi(m,0,1); 48 | *j_left = rot1 * j_right->transpose(); 49 | } 50 | 51 | } // end namespace internal 52 | 53 | } // end namespace Eigen 54 | 55 | #endif // EIGEN_REALSVD2X2_H 56 | -------------------------------------------------------------------------------- /src/slam/include/Eigen/src/misc/lapacke_mangling.h: -------------------------------------------------------------------------------- 1 | #ifndef LAPACK_HEADER_INCLUDED 2 | #define LAPACK_HEADER_INCLUDED 3 | 4 | #ifndef LAPACK_GLOBAL 5 | #if defined(LAPACK_GLOBAL_PATTERN_LC) || defined(ADD_) 6 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 7 | #elif defined(LAPACK_GLOBAL_PATTERN_UC) || defined(UPPER) 8 | #define LAPACK_GLOBAL(lcname,UCNAME) UCNAME 9 | #elif defined(LAPACK_GLOBAL_PATTERN_MC) || defined(NOCHANGE) 10 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname 11 | #else 12 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 13 | #endif 14 | #endif 15 | 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /src/slam/include/core/CameraStereoImages.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "core/StereoCameraModel.h" 8 | #include "core/SensorData.h" 9 | #include "core/Directory.h" 10 | #include "core/FPGA.h" 11 | #include "core/Parameters.h" 12 | 13 | class CameraStereoImages 14 | { 15 | public: 16 | CameraStereoImages( 17 | const std::string pathLeftImages = "", 18 | const std::string pathRightImages = "" 19 | ); 20 | ~CameraStereoImages(); 21 | 22 | bool init(int inputType); 23 | 24 | std::vector filenames() const; 25 | void captureFromFile(SensorData &data, APP_SETTING appSetting); 26 | void captureFile(const char *path, cv::Mat &image, int doResize); 27 | void captureFromFpga(Fpga *fpga, SensorData &data, APP_SETTING appSetting); 28 | 29 | void setTimestamps(const std::string & filePath) { _timestampsPath = filePath; } 30 | 31 | void setGroundTruthPath(const std::string & filePath) { _groundTruthPath = filePath; } 32 | 33 | 34 | protected: 35 | int getNextSeqID() { int tmp = _seq; _seq++; return tmp; } 36 | 37 | private: 38 | int _seq; 39 | std::string _timestampsPath; 40 | std::string _groundTruthPath; 41 | std::list groundTruth_; 42 | std::vector _stamps; 43 | int _frameNum; 44 | float _captureTime; 45 | 46 | // left imager 47 | std::string _path_l; 48 | Directory *_dir_l; 49 | 50 | // right imager 51 | std::string _path_r; 52 | Directory *_dir_r; 53 | }; 54 | -------------------------------------------------------------------------------- /src/slam/include/core/Directory.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | class Directory 12 | { 13 | public: 14 | Directory(const std::string & path = ""); 15 | ~Directory(); 16 | 17 | void update(const std::string & path); 18 | std::string getNextFileName(); 19 | const std::list & getFileNames() const {return fileNames_;} 20 | 21 | private: 22 | std::string path_; 23 | std::list fileNames_; 24 | std::list::iterator iFileName_; 25 | }; 26 | 27 | -------------------------------------------------------------------------------- /src/slam/include/core/EigenTypes.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "Eigen/Core" 8 | #include "Eigen/Geometry" 9 | #include 10 | #include 11 | 12 | typedef Eigen::Matrix Vector3D; 13 | typedef Eigen::Matrix Vector6D; 14 | typedef Eigen::Matrix Matrix3D; 15 | typedef Eigen::Matrix Matrix6D; 16 | typedef Eigen::Transform Isometry3; 17 | typedef Eigen::Quaternion Quaternion; 18 | 19 | // sparse linear system 20 | typedef Eigen::Matrix PoseMatrixType; 21 | typedef Eigen::Map HessianBlockType; 22 | typedef std::tuple HessianTuple; 23 | typedef std::stack>> BackupStackType; 24 | -------------------------------------------------------------------------------- /src/slam/include/core/FlannIndex.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | class FlannIndex 11 | { 12 | public: 13 | FlannIndex(); 14 | virtual ~FlannIndex(); 15 | 16 | void release(); 17 | unsigned int indexedFeatures(); 18 | void buildKDTreeIndex(const cv::Mat &features, int trees); 19 | bool isBuilt(); 20 | std::vector addPoints(const cv::Mat & features); 21 | void knnSearch(const cv::Mat &query, cv::Mat &indices, cv::Mat &dists, int knn, int checks); 22 | 23 | unsigned long getSize(); 24 | 25 | private: 26 | void *index_; 27 | unsigned int nextIndex_; 28 | int featuresType_; 29 | int featuresDim_; 30 | unsigned long featureSize_; 31 | std::map addedDescriptors_; 32 | }; 33 | 34 | -------------------------------------------------------------------------------- /src/slam/include/core/GFTT.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "opencv2/core/core.hpp" 8 | #include "opencv2/features2d/features2d.hpp" 9 | 10 | void generateKeypoints(cv::Mat &img, std::vector &kpts2d); 11 | void generateKeypoints2(cv::Mat &eig, unsigned short max, std::vector &kpts2d); 12 | -------------------------------------------------------------------------------- /src/slam/include/core/Graph.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | std::multimap::iterator findLink(std::multimap &links, int from, int to); 11 | 12 | bool importPoses(const std::string &filePath, std::map &poses); 13 | 14 | void savePoses(const char* filename, std::map poses); 15 | void saveLinks(const char* filename, std::multimap links); 16 | void loadPoses(const char *filename, std::map *poses); 17 | void loadLinks(const char* filename, std::multimap *links); 18 | -------------------------------------------------------------------------------- /src/slam/include/core/GraphEdge.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "Eigen/Sparse" 8 | #include "EigenTypes.h" 9 | #include "GraphVertex.h" 10 | 11 | class Edge 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 15 | 16 | Edge(); 17 | void setVertex(size_t i, Vertex* v); 18 | void setMeasurement(const Isometry3& m); 19 | void setInformation(const Matrix6D& information); 20 | std::vector& vertices() { return _vertices; } 21 | void setInternalId(int id) { _internalId = id; } 22 | int internalId() const { return _internalId; } 23 | 24 | void computeError(); 25 | double chi2(); 26 | void computeJacobian(); 27 | void constructQuadraticForm(double *b, std::vector> &coef, int iteration, double *max_diag); 28 | 29 | protected: 30 | std::vector _vertices; 31 | Isometry3 _measurement; 32 | Isometry3 _inverseMeasurement; 33 | Matrix6D _information; 34 | Vector6D _error; 35 | Matrix6D _jacobianOplus[2]; 36 | int _internalId; 37 | int _dimension; 38 | }; 39 | 40 | -------------------------------------------------------------------------------- /src/slam/include/core/GraphVertex.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "EigenTypes.h" 8 | 9 | class Vertex 10 | { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 13 | 14 | Vertex(); 15 | 16 | void setEstimate(const Isometry3 &et) { _estimate = et; } 17 | const Isometry3 &estimate() const { return _estimate; } 18 | void setFixed(bool fixed) { _fixed = fixed; } 19 | void setId(int newId) { _id = newId; } 20 | int id() const { return _id; } 21 | bool fixed() const { return _fixed; } 22 | void setHessianIndex(int ti) { _hessianIndex = ti; } 23 | int hessianIndex() const { return _hessianIndex; } 24 | 25 | void oplus(const double *update); 26 | 27 | protected: 28 | Isometry3 fromVectorMQT(const Vector6D& v); 29 | Matrix3D fromCompactQuaternion(const Vector3D& v); 30 | 31 | Isometry3 _estimate; 32 | int _id; 33 | bool _fixed; 34 | int _hessianIndex; 35 | }; 36 | 37 | -------------------------------------------------------------------------------- /src/slam/include/core/HyperGraph.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "Eigen/Sparse" 8 | #include "EigenTypes.h" 9 | #include "GraphVertex.h" 10 | #include "GraphEdge.h" 11 | 12 | class HyperGraph 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 16 | 17 | HyperGraph(); 18 | 19 | bool addVertex(Vertex *v); 20 | bool addEdge(Edge *e); 21 | Vertex *vertex(int id); 22 | 23 | void removeVertices(); 24 | void removeEdges(); 25 | 26 | int optimize(int iterations); 27 | void buildIndexMapping(); 28 | double computeActiveErrors(); 29 | void updateGraph(double *x); 30 | void buildSystem( 31 | int iteration, 32 | double *b, 33 | std::vector> &coef, 34 | double *max_diag); 35 | void solveEigen( 36 | int iteration, 37 | double *b, 38 | std::vector> &coef, 39 | double lambda, 40 | Eigen::VectorXd &eigen_x); 41 | double scaleLambda(double *x, double *b, double currentChi, double currentLambda); 42 | 43 | protected: 44 | std::map _vertices; 45 | std::vector _edges; 46 | int _dimension; 47 | 48 | int _numPoses; 49 | int _sizePoses; 50 | int _nextEdgeId; 51 | 52 | double _tau; 53 | double _lowerStep; 54 | double _upperStep; 55 | }; 56 | 57 | -------------------------------------------------------------------------------- /src/slam/include/core/Link.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | class Link 11 | { 12 | public: 13 | enum Type { 14 | Neighbor, 15 | LoopClosure, 16 | Undefined 17 | }; 18 | 19 | Link(); 20 | Link( 21 | int from, 22 | int to, 23 | Type type, 24 | const Transform &transform, 25 | const cv::Mat &infMatrix = cv::Mat::eye(6, 6, CV_64FC1)); 26 | 27 | void setFrom(int from) { from_ = from; } 28 | int from() const { return from_; } 29 | 30 | void setTo(int to) { to_ = to; } 31 | int to() const { return to_; } 32 | 33 | void setTransform(const Transform &transform) { transform_ = transform; } 34 | const Transform &transform() const { return transform_; } 35 | 36 | void setType(Type type) { type_ = type; } 37 | Type type() const { return type_; } 38 | 39 | void setInfMatrix(const cv::Mat &infMatrix) { infMatrix_ = infMatrix; } 40 | const cv::Mat &infMatrix() const { return infMatrix_; } 41 | 42 | bool isValid(); 43 | Link inverse() const; 44 | 45 | unsigned long getSize(); 46 | 47 | private: 48 | int from_; 49 | int to_; 50 | Transform transform_; 51 | Type type_; 52 | cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1 53 | }; 54 | 55 | -------------------------------------------------------------------------------- /src/slam/include/core/Logger.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | enum LOG_LEVEL { LOG_LEVEL_DEBUG, LOG_LEVEL_INFO, LOG_LEVEL_WARN, LOG_LEVEL_ERROR }; 11 | 12 | void log_write( 13 | LOG_LEVEL level, 14 | const char *file, 15 | int line, 16 | const char *function, 17 | const char *msg, 18 | ...); 19 | 20 | #define LOG_DEBUG(...) log_write(LOG_LEVEL_DEBUG, __FILE__, __LINE__, __FUNCTION__, __VA_ARGS__) 21 | #define LOG_INFO(...) log_write(LOG_LEVEL_INFO, __FILE__, __LINE__, __FUNCTION__, __VA_ARGS__) 22 | #define LOG_WARN(...) log_write(LOG_LEVEL_WARN, __FILE__, __LINE__, __FUNCTION__, __VA_ARGS__) 23 | #define LOG_ERROR(...) log_write(LOG_LEVEL_ERROR, __FILE__, __LINE__, __FUNCTION__, __VA_ARGS__) 24 | 25 | void clearLogFile(); 26 | void writeToLogFile(const char* filename); 27 | -------------------------------------------------------------------------------- /src/slam/include/core/Mapper.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "core/Node.h" 8 | #include "core/Transform.h" 9 | #include "core/VWDictionary.h" 10 | #include "core/Odometry.h" 11 | #include "core/xThread.h" 12 | #include "core/Parameters.h" 13 | 14 | void getConnectedGraph( 15 | int fromId, 16 | std::map & posesIn, 17 | std::multimap & linksIn, 18 | std::map & posesOut, 19 | std::multimap & linksOut); 20 | 21 | struct TH_PARAM { 22 | Node *node; 23 | VWDictionary *vwd; 24 | std::map *nodes; 25 | std::map *workingMem; 26 | Link link; 27 | int state; 28 | }; 29 | 30 | void* thread(void* arg); 31 | void* addWordIds(Node *node, VWDictionary *vwd); 32 | void detectLoopClosure(std::map &nodes, std::map &workingMem, VWDictionary *_vwd, int id, Link *link); 33 | std::map computeLikelihood(Node *node, std::map &_nodes, VWDictionary *_vwd, const std::list & ids); 34 | 35 | class Mapper 36 | { 37 | public: 38 | Mapper(); 39 | virtual ~Mapper(); 40 | 41 | bool process(SensorData &data, ODOM_INFO odomInfo, APP_SETTING appSetting); 42 | void init(); 43 | void getGraph(std::map &poses, std::multimap &links); 44 | cv::Mat getInformation(const cv::Mat & covariance) const; 45 | void cleanupThread(); 46 | bool updateMemory(Node *node, cv::Mat &covariance); 47 | bool addLink(const Link &link); 48 | const std::map &getWorkingMem() const { return _workingMem; } 49 | const std::set &getStMem() const { return _stMem; } 50 | std::multimap getLinks(int nodeId) const; 51 | Node *getLastNode(); 52 | const Node *getNode(int id) const; 53 | const VWDictionary *getVWDictionary() const; 54 | void getMemoryUsed(); 55 | const std::map &getNodes() const { return _nodes; } 56 | Node *_getNode(int id) const; 57 | void clearNodes(); 58 | 59 | private: 60 | void addNodeToStm(Node *node, const cv::Mat &covariance); 61 | void loadDataFromDb(bool postInitClosingEvents); 62 | void moveNodeToWMFromSTM(); 63 | int getNextId(); 64 | void initCountId(); 65 | Node *createNode(SensorData &data, ODOM_INFO odomInfo); 66 | 67 | int _frameProcessed; 68 | int _intermediateCount; 69 | int _mapUpdate; 70 | std::map _likelihood; 71 | int _key_id; 72 | int _maxStMemSize; 73 | int _idCount; 74 | int _idMapCount; // map id, reserved for multi-map session 75 | Node *_lastNode; 76 | std::map _nodes; 77 | std::set _stMem; // contains node IDs 78 | std::map _workingMem; // 79 | VWDictionary *_vwd; 80 | TH_PARAM _th_param; 81 | xThread _xth; 82 | }; 83 | -------------------------------------------------------------------------------- /src/slam/include/core/MotionEstimation.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "core/Transform.h" 8 | #include "opencv2/opencv.hpp" 9 | #include "core/StereoCameraModel.h" 10 | 11 | Transform estimateMotion3DTo2D( 12 | const std::map &words3A, 13 | const std::map &words2B, 14 | const StereoCameraModel &cameraModel, 15 | int minInliers, 16 | int refineIterations, 17 | const Transform &guess, 18 | const std::map &words3B, 19 | cv::Mat *covariance, // mean reproj error if words3B is not set 20 | std::vector *matchesOut, 21 | std::vector *inliersOut); 22 | 23 | std::vector computeReprojErrors( 24 | std::vector opoints, 25 | std::vector ipoints, 26 | const cv::Mat &cameraMatrix, 27 | const cv::Mat &distCoeffs, 28 | const cv::Mat &rvec, 29 | const cv::Mat &tvec, 30 | float threshold, 31 | std::vector &inliers); 32 | 33 | void solvePnPRansac( 34 | const std::vector &objectPoints, 35 | const std::vector &imagePoints, 36 | const cv::Mat &cameraMatrix, 37 | const cv::Mat &distCoeffs, 38 | cv::Mat &rvec, 39 | cv::Mat &tvec, 40 | int minInliersCount, 41 | int refineIterations, 42 | std::vector &inliers); 43 | 44 | -------------------------------------------------------------------------------- /src/slam/include/core/Node.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | class Node 16 | { 17 | 18 | public: 19 | Node(); 20 | Node(int id, int mapId, int weight, const Transform &pose, const SensorData &sensorData); 21 | Node(const SensorData & data); 22 | virtual ~Node(); 23 | 24 | int id() const { return _id; } 25 | int mapId() const { return _mapId; } 26 | 27 | void setWeight(int weight) { _weight = weight; } 28 | int getWeight() const { return _weight; } 29 | 30 | double getStamp() const { return _stamp; } 31 | 32 | void addLink(const Link & link); 33 | bool hasLink(int idTo, Link::Type type = Link::Undefined) const; 34 | const std::multimap & getLinks() const { return _links; } 35 | 36 | void setWords(std::multimap words) { _words = words; } 37 | const std::multimap & getWords() const { return _words; } 38 | 39 | void setVelocity(Transform velocity) { _velocity = velocity; } 40 | const Transform getVelocity() const { return _velocity; } 41 | 42 | SensorData &sensorData() { return _sensorData; } 43 | const SensorData &sensorData() const { return _sensorData; } 44 | 45 | const Transform & getPose() const { return _pose; } 46 | const Transform &groundTruth() const { return _groundTruth; } 47 | 48 | void getMemoryUsed(); 49 | 50 | private: 51 | int _id; 52 | int _mapId; // reserved for multi-map session 53 | double _stamp; 54 | std::multimap _links; // 55 | int _weight; 56 | 57 | std::multimap _words; // word 58 | 59 | Transform _pose; 60 | Transform _velocity; 61 | 62 | SensorData _sensorData; 63 | Transform _groundTruth; 64 | }; 65 | -------------------------------------------------------------------------------- /src/slam/include/core/Odometry.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "core/Transform.h" 8 | #include "core/SensorData.h" 9 | #include "core/Registration.h" 10 | 11 | struct ODOM_INFO { 12 | Transform pose; 13 | bool lost; 14 | double stamp; 15 | double interval; 16 | Transform velocity; 17 | Transform transform; 18 | float distanceTravelled; 19 | cv::Mat covariance; 20 | }; 21 | 22 | class Odometry 23 | { 24 | public: 25 | enum State { 26 | Initialized, 27 | Running, 28 | Lost 29 | }; 30 | 31 | Odometry(); 32 | ~Odometry(); 33 | void process(SensorData data, ODOM_INFO *odomInfo); 34 | 35 | const Transform &getPose() const { return _pose; } 36 | unsigned int framesProcessed() const { return framesProcessed_; } 37 | 38 | Transform updateMotion(SensorData data, const Transform guess); 39 | 40 | void setNumObjects(int numObjects) { _numObjects = numObjects; } 41 | int getNumObjects() { return _numObjects; } 42 | 43 | REG_INFO regInfo() { return _regInfo; } 44 | 45 | void getMemoryUsed(); 46 | 47 | private: 48 | Transform _pose; // current pose 49 | double previousStamp_; 50 | Transform velocityGuess_; 51 | unsigned int framesProcessed_; 52 | Transform lastKeyFramePose_; 53 | SensorData refFrame_; 54 | int _numObjects; 55 | float _guessRatio; 56 | int _numFeatures; 57 | bool _keyFrameAdded; 58 | float _distanceTravelled; 59 | REG_INFO _regInfo; 60 | int _state; 61 | }; 62 | -------------------------------------------------------------------------------- /src/slam/include/core/Optimizer.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "core/Transform.h" 8 | #include "core/Link.h" 9 | #include "core/HyperGraph.h" 10 | #include "core/GraphVertex.h" 11 | #include "core/GraphEdge.h" 12 | 13 | void addVertices( 14 | HyperGraph &graph, 15 | std::map poses, 16 | std::list &vertices); 17 | 18 | void addEdges( 19 | HyperGraph &graph, 20 | std::multimap links, 21 | std::multimap &edges); 22 | 23 | double runOptimize( 24 | std::map poses, 25 | std::multimap links, 26 | int num, 27 | std::map *optimized_poses); 28 | 29 | double runOptimizeRobust( 30 | std::map poses, 31 | std::multimap links, 32 | int num, 33 | std::map *optimized_poses); 34 | -------------------------------------------------------------------------------- /src/slam/include/core/Perf.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | float currentTimeMs(); 13 | float currentTimeSec(); 14 | 15 | class Perf 16 | { 17 | public: 18 | Perf(); 19 | virtual ~Perf(); 20 | 21 | void startTime(std::string functionName); 22 | void stopTime(std::string functionName); 23 | void addTimeLog(std::string functionName); 24 | void write(const char* filename); 25 | void setFrameId(int frameId) { _frameId = frameId; } 26 | float elapsedTimeMs(); 27 | 28 | int retrieveMemoryId(std::string functionName); 29 | std::string retrieveMemoryName(int id); 30 | void registerMemoryUsed(std::string functionName, unsigned long memoryUsed); 31 | void writeMemoryUsed(const char* filename); 32 | 33 | private: 34 | void registerTime(int functionId); 35 | int retrieveId(std::string functionName); 36 | std::string retrieveName(int id); 37 | 38 | std::mutex _mutex; 39 | 40 | int _frameId; 41 | float _initialTime; 42 | std::vector _timer; 43 | std::map _functionNameList; // 44 | std::map> _procTime; // 45 | 46 | std::map _memoryNameList; // 47 | std::map> _memoryUsed; // 48 | }; 49 | -------------------------------------------------------------------------------- /src/slam/include/core/Registration.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "flann/flann.hpp" 8 | #include "opencv/CvLKStereo.h" 9 | #include "core/Node.h" 10 | #include "core/VisualWord.h" 11 | #include "core/MotionEstimation.h" 12 | #include "core/Logger.h" 13 | #include "core/Stereo.h" 14 | 15 | struct REG_INFO { 16 | cv::Mat covariance; 17 | int num_inliers; 18 | int num_matches; 19 | }; 20 | 21 | Transform computeTransform( 22 | SensorData sensorFrom, 23 | SensorData sensorTo, 24 | Transform guess, 25 | struct REG_INFO *info); 26 | 27 | void matchingGuess_Projection( 28 | std::vector kptsFrom3D, 29 | std::vector &projectedPoint, 30 | std::vector &projectedIndex, 31 | StereoCameraModel cameraModel, 32 | Transform guess); 33 | 34 | void matchingGuess_radiusSearch( 35 | std::vector kptsTo, 36 | std::vector projectedPoint, 37 | std::vector> &indices); 38 | 39 | void matchingGuess_Nndr( 40 | cv::Mat descriptorsFrom, 41 | cv::Mat descriptorsTo, 42 | std::vector projectedIndex, 43 | std::vector projectedPoint, 44 | std::vector> indices, 45 | std::multimap &matchedIndex); 46 | 47 | int matchingGuess_search( 48 | cv::Mat descriptorFrom, 49 | cv::Mat descriptorsTo, 50 | std::vector indices); 51 | 52 | void matchingGuess( 53 | SensorData sensorFrom, 54 | SensorData sensorTo, 55 | Transform guess, 56 | std::multimap &matchedIndex); 57 | 58 | void matchingNoGuess( 59 | SensorData sensorFrom, 60 | SensorData sensorTo, 61 | std::multimap &matchedIndex); 62 | 63 | void estimateMotion( 64 | SensorData sensorFrom, 65 | SensorData sensorTo, 66 | Transform guess, 67 | Transform &transform, 68 | REG_INFO *reg_info, 69 | std::multimap matchedIndex); 70 | -------------------------------------------------------------------------------- /src/slam/include/core/Stereo.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include "core/StereoCameraModel.h" 9 | #include "core/SensorData.h" 10 | 11 | std::vector computeCorrespondences( 12 | const cv::Mat &leftImage, 13 | const cv::Mat &rightImage, 14 | const std::vector &leftCorners, 15 | std::vector &status); 16 | 17 | std::vector generateKeypoints3DStereo( 18 | const std::vector &leftCorners, 19 | const std::vector &rightCorners, 20 | const StereoCameraModel &model, 21 | const std::vector &mask, 22 | float minDepth, 23 | float maxDepth, 24 | cv::Mat disp, 25 | int depthMethod); 26 | 27 | void generateKeypoints3D( 28 | SensorData data, 29 | StereoCameraModel StereoCameraModel, 30 | std::vector kpts, 31 | std::vector &kpts3d, 32 | cv::Mat disp, 33 | int depthMethod); 34 | 35 | cv::Point3f projectDisparityTo3D( 36 | const cv::Point2f &pt2d, 37 | float disp, 38 | const StereoCameraModel &model); 39 | 40 | bool isFinite(const cv::Point3f &pt); 41 | 42 | cv::Point3f transformPoint( 43 | const cv::Point3f &point, 44 | const Transform &transform); 45 | 46 | -------------------------------------------------------------------------------- /src/slam/include/core/StereoCameraModel.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "opencv2/opencv.hpp" 8 | #include "core/Transform.h" 9 | 10 | class StereoCameraModel 11 | { 12 | public: 13 | StereoCameraModel(); 14 | ~StereoCameraModel(); 15 | 16 | double baseline() const; 17 | bool load( 18 | const std::string &fileNameLeft, 19 | const std::string &fileNameRight, 20 | int doResize); 21 | 22 | const Transform & localTransform() const { return _localTransform; } 23 | const cv::Size & imageSize() const { return _imageSize; } 24 | 25 | double fx_l() const { return _P[0].at(0, 0); } 26 | double fy_l() const { return _P[0].at(1, 1); } 27 | double cx_l() const { return _P[0].at(0, 2); } 28 | double cy_l() const { return _P[0].at(1, 2); } 29 | double Tx_l() const { return _P[0].at(0, 3); } 30 | double fx_r() const { return _P[1].at(0, 0); } 31 | double fy_r() const { return _P[1].at(1, 1); } 32 | double cx_r() const { return _P[1].at(0, 2); } 33 | double cy_r() const { return _P[1].at(1, 2); } 34 | double Tx_r() const { return _P[1].at(0, 3); } 35 | cv::Mat K_l() const { return _P[0].colRange(0, 3); } // 3x3 camera matrix 36 | cv::Mat D_l() const { return cv::Mat::zeros(1, 5, CV_64FC1); } 37 | 38 | unsigned long getSize(); 39 | 40 | private: 41 | cv::Size _imageSize; 42 | cv::Mat _P[2]; 43 | Transform _localTransform; 44 | }; 45 | 46 | -------------------------------------------------------------------------------- /src/slam/include/core/Transform.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include "Eigen/Core" 11 | #include "Eigen/Geometry" 12 | #include "opencv2/core/core.hpp" 13 | 14 | class Transform 15 | { 16 | public: 17 | // Zero by default 18 | Transform(); 19 | 20 | // rotation matrix r## and origin o## 21 | Transform( 22 | float r11, float r12, float r13, float o14, 23 | float r21, float r22, float r23, float o24, 24 | float r31, float r32, float r33, float o34); 25 | Transform( 26 | double r11, double r12, double r13, double o14, 27 | double r21, double r22, double r23, double o24, 28 | double r31, double r32, double r33, double o34); 29 | 30 | // should have 3 rows, 4 cols and type CV_32FC1 31 | Transform(const cv::Mat & mat); 32 | 33 | // x,y,z, roll,pitch,yaw 34 | Transform(float x, float y, float z, float roll, float pitch, float yaw); 35 | 36 | ~Transform(); 37 | 38 | float r11() const { return data_.at(0, 0); } 39 | float r12() const { return data_.at(0, 1); } 40 | float r13() const { return data_.at(0, 2); } 41 | float o14() const { return data_.at(0, 3); } 42 | float r21() const { return data_.at(1, 0); } 43 | float r22() const { return data_.at(1, 1); } 44 | float r23() const { return data_.at(1, 2); } 45 | float o24() const { return data_.at(1, 3); } 46 | float r31() const { return data_.at(2, 0); } 47 | float r32() const { return data_.at(2, 1); } 48 | float r33() const { return data_.at(2, 2); } 49 | float o34() const { return data_.at(2, 3); } 50 | float & x() { return data_.at(0, 3); } 51 | float & y() { return data_.at(1, 3); } 52 | float & z() { return data_.at(2, 3); } 53 | const float & x() const { return data_.at(0, 3); } 54 | const float & y() const { return data_.at(1, 3); } 55 | const float & z() const { return data_.at(2, 3); } 56 | 57 | bool isNull() const; 58 | void setNull(); 59 | static Transform getIdentity(); 60 | Transform inverse() const; 61 | void getAngles(float & roll, float & pitch, float & yaw) const; 62 | float getNorm() const; 63 | 64 | static Transform fromEigen4f(const Eigen::Matrix4f & matrix); 65 | static Transform fromEigen3f(const Eigen::Affine3f & matrix); 66 | Eigen::Matrix4f toEigen4f() const; 67 | 68 | Transform operator*(const Transform & t) const; 69 | Transform & operator*=(const Transform & t); 70 | 71 | unsigned long getSize(); 72 | 73 | private: 74 | cv::Mat data_; 75 | }; 76 | -------------------------------------------------------------------------------- /src/slam/include/core/VWDictionary.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include "core/VisualWord.h" 8 | #include "core/FlannIndex.h" 9 | 10 | class VWDictionary 11 | { 12 | public: 13 | VWDictionary(); 14 | ~VWDictionary(); 15 | std::list addNewWords(const cv::Mat descriptors, int nodeId); 16 | const VisualWord* getWord(int id) const; 17 | void clear(); 18 | void getMemoryUsed(); 19 | 20 | protected: 21 | int getNextId(); 22 | std::map _visualWords; // 23 | 24 | private: 25 | int _lastWordId; 26 | FlannIndex *_flannIndex; 27 | std::map _mapIndexId; // of all VWs in the tree 28 | }; 29 | -------------------------------------------------------------------------------- /src/slam/include/core/VisualWord.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | class VisualWord 11 | { 12 | public: 13 | VisualWord(int id, const cv::Mat &descriptor, int nodeId = 0); 14 | ~VisualWord(); 15 | 16 | void addRef(int nodeId); 17 | int removeAllRef(int nodeId); 18 | 19 | int getTotalReferences() const { return _totalReferences; } 20 | int id() const { return _id; } 21 | const std::map &getReferences() const { return _references; } // (node id , occurrence in the node) 22 | unsigned long getSize(); 23 | 24 | private: 25 | int _id; 26 | int _totalReferences; 27 | std::map _references; // (node id , occurrence in the node) 28 | }; 29 | 30 | -------------------------------------------------------------------------------- /src/slam/include/core/xThread.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #pragma once 6 | 7 | #ifdef _WIN32 8 | #include 9 | #else 10 | #include 11 | #endif 12 | 13 | class xThread 14 | { 15 | public: 16 | xThread(); 17 | ~xThread(); 18 | 19 | int create(void*(*func)(void*), void *arg); 20 | int join(void); 21 | int lowerProirity(void); 22 | 23 | private: 24 | #ifdef _WIN32 25 | std::thread _th; 26 | #else 27 | pthread_t _th; 28 | #endif 29 | 30 | }; 31 | -------------------------------------------------------------------------------- /src/slam/include/flann/config.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2008-2011 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 5 | * Copyright 2008-2011 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in the 15 | * documentation and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | *************************************************************************/ 28 | 29 | 30 | #ifndef RTABMAP_FLANN_CONFIG_H_ 31 | #define RTABMAP_FLANN_CONFIG_H_ 32 | 33 | #ifdef FLANN_VERSION_ 34 | #undef FLANN_VERSION_ 35 | #endif 36 | #define FLANN_VERSION_ "1.8.4" 37 | 38 | #endif /* RTABMAP_FLANN_CONFIG_H_ */ 39 | -------------------------------------------------------------------------------- /src/slam/include/g2o/SE3Gradient.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 | // Copied from g2o, and modified by Nu-Gate Technology in accordance 28 | // with the above license. 29 | 30 | #pragma once 31 | 32 | #include "core/EigenTypes.h" 33 | 34 | namespace SE3 35 | { 36 | using number_t = double; 37 | using Matrix3 = Eigen::Matrix; 38 | 39 | void computeEdgeSE3Gradient( 40 | Matrix6D& JiConstRef, 41 | Matrix6D& JjConstRef, 42 | const Isometry3& Z, 43 | const Isometry3& Xi, 44 | const Isometry3& Xj 45 | ); 46 | void compute_dq_dR( 47 | Eigen::Matrix& dq_dR, 48 | const double& r11, const double& r21, const double& r31, 49 | const double& r12, const double& r22, const double& r32, 50 | const double& r13, const double& r23, const double& r33); 51 | int _q2m( 52 | double& S, double& qw, 53 | const double& r00, const double& r10, const double& r20, 54 | const double& r01, const double& r11, const double& r21, 55 | const double& r02, const double& r12, const double& r22); 56 | void compute_dq_dR_w( 57 | Eigen::Matrix& dq_dR_w, const double& qw, 58 | const double& r00, const double& r10, const double& r20, 59 | const double& r01, const double& r11, const double& r21, 60 | const double& r02, const double& r12, const double& r22); 61 | void skew1(Matrix3& s, const Isometry3::ConstTranslationPart& v, bool transposed); 62 | void skew2(Matrix3& Sx, Matrix3& Sy, Matrix3& Sz, const Matrix3& R, bool transposed); 63 | }; 64 | 65 | -------------------------------------------------------------------------------- /src/slam/include/octomap/OcTreeBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * https://octomap.github.io/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef OCTOMAP_OCTREE_BASE_H 35 | #define OCTOMAP_OCTREE_BASE_H 36 | 37 | 38 | #include "OcTreeBaseImpl.h" 39 | #include "AbstractOcTree.h" 40 | 41 | 42 | namespace octomap { 43 | template 44 | class OcTreeBase : public OcTreeBaseImpl { 45 | public: 46 | OcTreeBase(double res) : OcTreeBaseImpl(res) {}; 47 | 48 | /// virtual constructor: creates a new object of same type 49 | /// (Covariant return type requires an up-to-date compiler) 50 | OcTreeBase* create() const {return new OcTreeBase(this->resolution); } 51 | std::string getTreeType() const {return "OcTreeBase";} 52 | }; 53 | 54 | } 55 | 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /src/slam/include/octomap/math/Utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * https://octomap.github.io/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef OCTOMATH_UTILS_H 35 | #define OCTOMATH_UTILS_H 36 | 37 | #ifndef M_PI 38 | #define M_PI 3.14159265358979323846 39 | #endif 40 | 41 | #ifndef M_PI_2 42 | #define M_PI_2 1.570796326794896619 43 | #endif 44 | 45 | 46 | #ifndef DEG2RAD 47 | #define DEG2RAD(x) ((x) * 0.01745329251994329575) 48 | #endif 49 | 50 | #ifndef RAD2DEG 51 | #define RAD2DEG(x) ((x) * 57.29577951308232087721) 52 | #endif 53 | 54 | 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/slam/include/octomap/octomap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * https://octomap.github.io/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include "octomap_types.h" 35 | #include "Pointcloud.h" 36 | #include "ScanGraph.h" 37 | #include "OcTree.h" 38 | 39 | -------------------------------------------------------------------------------- /src/slam/include/octomap/octomap_deprecated.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * https://octomap.github.io/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef OCTOMAP_DEPRECATED_H 35 | #define OCTOMAP_DEPRECATED_H 36 | 37 | // define multi-platform deprecation mechanism 38 | #ifndef OCTOMAP_DEPRECATED 39 | #ifdef __GNUC__ 40 | #define OCTOMAP_DEPRECATED(func) func __attribute__ ((deprecated)) 41 | #elif defined(_MSC_VER) 42 | #define OCTOMAP_DEPRECATED(func) __declspec(deprecated) func 43 | #else 44 | #pragma message("WARNING: You need to implement OCTOMAP_DEPRECATED for this compiler") 45 | #define OCTOMAP_DEPRECATED(func) func 46 | #endif 47 | #endif 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/slam/include/octomap/octomap_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * https://octomap.github.io/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef OCTOMAP_UTILS_H_ 35 | #define OCTOMAP_UTILS_H_ 36 | 37 | #include 38 | 39 | namespace octomap{ 40 | 41 | /// compute log-odds from probability: 42 | inline float logodds(double probability){ 43 | return (float) log(probability/(1-probability)); 44 | } 45 | 46 | /// compute probability from logodds: 47 | inline double probability(double logodds){ 48 | return 1. - ( 1. / (1. + exp(logodds))); 49 | 50 | } 51 | } 52 | 53 | 54 | 55 | #endif /* OCTOMAP_UTILS_H_ */ 56 | -------------------------------------------------------------------------------- /src/slam/include/opencv/CvLKStereo.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include 5 | #include 6 | 7 | // exactly as cv::calcOpticalFlowPyrLK but it should be called with pyramid (from cv::buildOpticalFlowPyramid()) and delta drops the y error. 8 | void calcOpticalFlowPyrLKStereo( 9 | cv::InputArray _prevImg, 10 | cv::InputArray _nextImg, 11 | cv::InputArray _prevPts, 12 | cv::InputOutputArray _nextPts, 13 | cv::OutputArray _status, 14 | cv::OutputArray _err, 15 | cv::Size winSize = cv::Size(15, 3), 16 | int maxLevel = 3, 17 | cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), 18 | int flags = 0, 19 | double minEigThreshold = 1e-4); 20 | -------------------------------------------------------------------------------- /src/slam/include/rtabmap/KITTI.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke 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 met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the Universite de Sherbrooke nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without 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 HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | 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 23 | ON 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 | // Copied from RTAB-Map, and modified by Nu-Gate Technology 29 | // in accordance with the above license. 30 | 31 | #pragma once 32 | 33 | #include 34 | 35 | namespace graph 36 | { 37 | void calcKittiSequenceErrors( 38 | const std::vector &poses_gt, 39 | const std::vector &poses_result, 40 | float & t_err, 41 | float & r_err); 42 | } 43 | -------------------------------------------------------------------------------- /src/slam/src/core/GraphVertex.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #include "core/GraphVertex.h" 6 | 7 | Vertex::Vertex() 8 | { 9 | _estimate = Isometry3::Identity(); 10 | _id = 0; 11 | _fixed = false; 12 | _hessianIndex = 0; 13 | } 14 | 15 | // update pose in the original parameter space 16 | void Vertex::oplus(const double *update) 17 | { 18 | // convert to original parameter space 19 | Eigen::Map v(update); 20 | 21 | Isometry3 increment; 22 | increment = fromCompactQuaternion(v.block<3, 1>(3, 0)); 23 | increment.translation() = v.block<3, 1>(0, 0); 24 | 25 | // update pose 26 | _estimate = _estimate * increment; 27 | } 28 | 29 | // compact quaternion -> rotation matrix 30 | Matrix3D Vertex::fromCompactQuaternion(const Vector3D& v) { 31 | double w = 1 - v.squaredNorm(); 32 | if (w < 0) { 33 | return Matrix3D::Identity(); 34 | } 35 | else { 36 | w = sqrt(w); 37 | } 38 | return Quaternion(w, v[0], v[1], v[2]).toRotationMatrix(); 39 | } 40 | -------------------------------------------------------------------------------- /src/slam/src/core/Link.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #include "core/Link.h" 6 | #include "core/Logger.h" 7 | 8 | Link::Link() 9 | { 10 | from_ = 0; 11 | to_ = 0; 12 | type_ = Undefined; 13 | infMatrix_ = cv::Mat::eye(6, 6, CV_64FC1); 14 | } 15 | 16 | Link::Link( 17 | int from, 18 | int to, 19 | Type type, 20 | const Transform &transform, 21 | const cv::Mat &infMatrix) 22 | { 23 | from_ = from; 24 | to_ = to; 25 | transform_ = transform; 26 | type_ = type; 27 | setInfMatrix(infMatrix); 28 | } 29 | 30 | bool Link::isValid() 31 | { 32 | bool valid = (from_ != 0) && (to_ != 0) && !transform_.isNull() && (type_ != Undefined); 33 | 34 | return valid; 35 | } 36 | 37 | Link Link::inverse() const 38 | { 39 | Link link( 40 | to_, 41 | from_, 42 | type_, 43 | transform_.isNull() ? Transform() : transform_.inverse(), 44 | transform_.isNull() ? cv::Mat::eye(6, 6, CV_64FC1) : infMatrix_); 45 | 46 | return link; 47 | } 48 | 49 | unsigned long Link::getSize() { 50 | unsigned long memUsed = (unsigned long)( 51 | sizeof(Link) + 52 | transform_.getSize() + 53 | infMatrix_.total() * infMatrix_.elemSize()); 54 | 55 | return memUsed; 56 | } 57 | -------------------------------------------------------------------------------- /src/slam/src/core/Logger.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #include "core/Logger.h" 6 | #include "core/Parameters.h" 7 | #include 8 | #include 9 | 10 | extern APP_SETTING appSetting; 11 | std::mutex loggerMutex_; 12 | std::vector messages_; 13 | 14 | void log_write( 15 | LOG_LEVEL level, 16 | const char *file, 17 | int line, 18 | const char *function, 19 | const char *msg, 20 | ...) 21 | { 22 | loggerMutex_.lock(); 23 | 24 | va_list args; 25 | va_start(args, msg); 26 | 27 | // show messages 28 | if (!appSetting.quiet) { 29 | if (level == LOG_LEVEL_WARN) { 30 | printf(" [WARNING %s line%d %s] ", file, line, function); 31 | } 32 | else if (level == LOG_LEVEL_ERROR) { 33 | printf(" [ERROR %s line%d %s] ", file, line, function); 34 | } 35 | 36 | if (level != LOG_LEVEL_DEBUG) { 37 | vprintf(msg, args); 38 | } 39 | } 40 | 41 | // save log messages, they will be written to a file later 42 | int len = vsnprintf(NULL, 0, msg, args); 43 | std::vector buf(len + 1); 44 | vsnprintf(&buf[0], len + 1, msg, args); 45 | std::string str(&buf[0], &buf[0] + len); 46 | messages_.push_back(str); 47 | 48 | va_end(args); 49 | 50 | loggerMutex_.unlock(); 51 | 52 | // halt on error 53 | if (level == LOG_LEVEL_ERROR) { 54 | while (1) {} 55 | } 56 | } 57 | 58 | void writeToLogFile(const char* filename) { 59 | FILE *fp_log = fopen(filename, "w"); 60 | if (fp_log != 0) { 61 | for (int i = 0; i < (int)messages_.size(); i++) { 62 | fprintf(fp_log, "%s", messages_[i].c_str()); 63 | } 64 | messages_.clear(); 65 | messages_.shrink_to_fit(); 66 | fclose(fp_log); 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /src/slam/src/core/Node.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #include "core/Node.h" 6 | #include "core/Perf.h" 7 | #include 8 | 9 | extern Perf perf; 10 | 11 | Node::Node() 12 | { 13 | _id = 0; 14 | _mapId = -1; 15 | _stamp = 0.0; 16 | _weight = 0; 17 | } 18 | 19 | Node::Node( 20 | int id, 21 | int mapId, 22 | int weight, 23 | const Transform & pose, 24 | const SensorData & sensorData) 25 | { 26 | _id = id; 27 | _mapId = mapId; 28 | _stamp = sensorData.stamp(); 29 | _weight = weight; 30 | _pose = pose; 31 | _sensorData = sensorData; 32 | 33 | _groundTruth = sensorData.groundTruth(); 34 | } 35 | 36 | Node::~Node() 37 | { 38 | } 39 | 40 | void Node::addLink(const Link & link) 41 | { 42 | _links.insert(std::make_pair(link.to(), link)); 43 | } 44 | 45 | bool Node::hasLink(int idTo, Link::Type type) const 46 | { 47 | if (type == Link::Undefined) { 48 | return _links.find(idTo) != _links.end(); 49 | } 50 | if (idTo == 0) { 51 | for (auto iter = _links.begin(); iter != _links.end(); ++iter) { 52 | if (type == iter->second.type()) { 53 | return true; 54 | } 55 | } 56 | } 57 | else { 58 | for (auto iter = _links.find(idTo); iter != _links.end() && iter->first == idTo; ++iter) { 59 | if (type == iter->second.type()) { 60 | return true; 61 | } 62 | } 63 | } 64 | 65 | return false; 66 | } 67 | 68 | void Node::getMemoryUsed() 69 | { 70 | unsigned long memUsed = (unsigned long)( 71 | sizeof(Node) + 72 | _pose.getSize() * 3 + 73 | _words.size() * (12 + sizeof(int) + sizeof(int))); 74 | 75 | if (_links.size() > 0) { 76 | memUsed += (unsigned long)(_links.size() * (12 + sizeof(int) + _links.begin()->second.getSize())); 77 | } 78 | 79 | perf.registerMemoryUsed("Node", memUsed); 80 | 81 | // SensorData 82 | _sensorData.getMemoryUsed(); 83 | } 84 | -------------------------------------------------------------------------------- /src/slam/src/core/VisualWord.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #include "core/VisualWord.h" 6 | #include "core/Logger.h" 7 | 8 | VisualWord::VisualWord(int id, const cv::Mat &descriptor, int nodeId) 9 | { 10 | _id = id; 11 | _totalReferences = 0; 12 | _references.clear(); 13 | 14 | if (nodeId) 15 | { 16 | addRef(nodeId); 17 | } 18 | } 19 | 20 | VisualWord::~VisualWord() 21 | { 22 | } 23 | 24 | void VisualWord::addRef(int nodeId) 25 | { 26 | auto iter = _references.find(nodeId); 27 | if (iter != _references.end()) 28 | { 29 | (*iter).second += 1; 30 | } 31 | else 32 | { 33 | _references.insert(std::pair(nodeId, 1)); 34 | } 35 | ++_totalReferences; 36 | } 37 | 38 | int VisualWord::removeAllRef(int nodeId) 39 | { 40 | int removed; 41 | auto itr = _references.find(nodeId); 42 | if (itr != _references.end()) { 43 | removed = itr->second; 44 | _references.erase(itr); 45 | } 46 | else { 47 | removed = 0; 48 | } 49 | 50 | _totalReferences -= removed; 51 | return removed; 52 | } 53 | 54 | unsigned long VisualWord::getSize() 55 | { 56 | unsigned long memUsed = (unsigned long)( 57 | sizeof(VisualWord) + 58 | sizeof(std::map) + 59 | _references.size() * (12 + sizeof(int) + sizeof(int))); 60 | 61 | return memUsed; 62 | } 63 | -------------------------------------------------------------------------------- /src/slam/src/core/xThread.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright (C) 2023 Nu-Gate Technology. All rights reserved. 3 | // SPDX-License-Identifier: MIT 4 | //============================================================================= 5 | #include "core/xThread.h" 6 | #include "core/Logger.h" 7 | 8 | #ifdef _WIN32 9 | #include 10 | #endif 11 | 12 | xThread::xThread() {} 13 | xThread::~xThread() {} 14 | 15 | int xThread::create(void*(*func)(void*), void *arg) 16 | { 17 | #ifdef _WIN32 18 | _th = std::thread(func, arg); 19 | return 0; 20 | #else 21 | int ret = pthread_create(&_th, NULL, func, arg); 22 | if (ret != 0) { 23 | LOG_WARN("pthread_create failed[%d]", ret); 24 | return -1; 25 | } 26 | return 0; 27 | #endif 28 | } 29 | 30 | int xThread::join() 31 | { 32 | #ifdef _WIN32 33 | _th.join(); 34 | return 0; 35 | #else 36 | void *status; 37 | int ret = pthread_join(_th, &status); 38 | if (ret != 0) { 39 | LOG_WARN("pthread_join failed[%d,%d]", ret, status); 40 | return -1; 41 | } 42 | return 0; 43 | #endif 44 | } 45 | 46 | int xThread::lowerProirity(void) 47 | { 48 | #if defined(_WIN32) 49 | int result = SetThreadPriority( 50 | reinterpret_cast(_th.native_handle()), 51 | THREAD_PRIORITY_BELOW_NORMAL); 52 | if (result != 0) { 53 | LOG_WARN("Setting priority number failed with %d", GetLastError()); 54 | return -1; 55 | } 56 | return 0; 57 | #else 58 | // NOTE:default priority is 0. below code has no effect. 59 | // get default thread attribute 60 | pthread_attr_t attr; 61 | int ret = pthread_attr_init(&attr); 62 | if (ret != 0) { 63 | LOG_WARN("pthread_attr_init failed[%d]", ret); 64 | return -1; 65 | } 66 | 67 | // get scheduling parameter 68 | sched_param param; 69 | ret = pthread_attr_getschedparam(&attr, ¶m); 70 | if (ret != 0) { 71 | LOG_WARN("pthread_attr_getschedparam failed[%d]", ret); 72 | return -1; 73 | } 74 | 75 | // lower the priority 76 | if (param.sched_priority > 0) { 77 | param.sched_priority -= 1; 78 | 79 | // set new scheduling parameter 80 | ret = pthread_attr_setschedparam(&attr, ¶m); 81 | if (ret != 0) { 82 | LOG_WARN("pthread_attr_setschedparam failed[%d]", ret); 83 | return -1; 84 | } 85 | } 86 | 87 | return 0; 88 | #endif 89 | } 90 | -------------------------------------------------------------------------------- /src/slam/src/octomap/OcTree.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 3 | * https://octomap.github.io/ 4 | * 5 | * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg 6 | * All rights reserved. 7 | * License: New BSD 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the distribution. 17 | * * Neither the name of the University of Freiburg nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | 36 | 37 | namespace octomap { 38 | 39 | OcTree::OcTree(double in_resolution) 40 | : OccupancyOcTreeBase(in_resolution) { 41 | ocTreeMemberInit.ensureLinking(); 42 | } 43 | 44 | OcTree::OcTree(std::string _filename) 45 | : OccupancyOcTreeBase (0.1) { // resolution will be set according to tree file 46 | readBinary(_filename); 47 | } 48 | 49 | OcTree::StaticMemberInitializer OcTree::ocTreeMemberInit; 50 | 51 | 52 | 53 | 54 | } // namespace 55 | -------------------------------------------------------------------------------- /title_600_450.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sdoira/U96-SLAM/8e5395adc5f5ae3b12e1629e6da33dfc9ff760b9/title_600_450.png --------------------------------------------------------------------------------