├── .gitignore ├── .gitmodules ├── Readme.md ├── config.yaml ├── dds_rostopic ├── CMakeLists.txt ├── package.xml └── src │ └── dds_rostopic_node.cpp ├── fusion_estimator ├── CMakeLists.txt ├── cfg │ └── go2_description.urdf ├── msg │ └── FusionEstimatorTest.msg ├── package.xml └── src │ ├── GO2FusionEstimator │ ├── .gitignore │ ├── .gitmodules │ ├── Estimator │ │ ├── Cpp_Estimators │ │ │ ├── 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 │ │ │ ├── Estimator2001_Kalman.cpp │ │ │ ├── Estimator2001_Kalman.h │ │ │ ├── Estimator2002_UnscentedKalmanEstiamtor.cpp │ │ │ └── Estimator2002_UnscentedKalmanEstiamtor.h │ │ ├── EstimatorPortN.h │ │ ├── Readme.md │ │ ├── StateSpaceModel1.c │ │ └── StateSpaceModel2.c │ ├── Readme.md │ ├── SensorBase.cpp │ ├── SensorBase.h │ ├── Sensor_IMU.cpp │ ├── Sensor_IMU.h │ ├── Sensor_Legs.cpp │ └── Sensor_Legs.h │ └── fusion_estimator_node.cpp ├── message_handle ├── message_handle │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-310.pyc │ │ ├── message_handle.cpython-310.pyc │ │ ├── message_handle_node.cpython-310.pyc │ │ └── pointcloud_to_laserscan_custom.cpython-310.pyc │ └── message_handle_node.py ├── package.xml ├── resource │ └── message_handle ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── other ├── Guide.yaml ├── PlotjugglerConf1.xml ├── SMXFE_odm.rviz └── slam_params.yaml └── sport_control ├── CMakeLists.txt ├── launch ├── camera_launch.py ├── g1_launch.py ├── go2_launch.py ├── nav_launch.py ├── rviz_launch.py └── slam_launch.py ├── package.xml └── src └── sport_control_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | local_file/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/fusion_estimator/src/GO2FusionEstimator"] 2 | path = src/fusion_estimator/src/GO2FusionEstimator 3 | url = https://github.com/ShineMinxing/GO2FusionEstimator.git 4 | [submodule "src/unitree_sdk2"] 5 | path = src/unitree_sdk2 6 | url = git@github.com:unitreerobotics/unitree_sdk2.git 7 | [submodule "unitree_sdk2"] 8 | path = unitree_sdk2 9 | url = git@github.com:unitreerobotics/unitree_sdk2.git 10 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | # Ros2Go2Estimator 🦾 2 | [![License](https://img.shields.io/badge/License-MIT-blue.svg)](LICENSE) 3 | 4 | - 一种高精度里程计解决方案, 5 | - 基于纯运动学的双足/四足机器人位置估计算法，目前仅使用IMU、足压力传感器、关节角度和角速度，不依赖相机或Lidar，但可将信号融合进去，进一步提高估计精度; 6 | - 使用config.yaml进行话题名称设置. 7 | 8 | ## 📚 补充说明 9 | - 切换两足、四足无需在估计器内做模式切换; 10 | - 目前没有调整参数做补偿，工程使用时可进一步提升精度; 11 | - dds_rostopic包将宇树dds提供的信息转换和发布为标准ros2话题; 12 | - fusion_estimator包发布对应“base_link”的话题SMX/Odom和对应“base_link_2D”的话题SMX/Odom_2D; 13 | - message_handle包完成SMX/Odom和SMX/Odom_2D的tf，此外，将frame“utlidar_lidar”的pointcloud2转换为“base_link_2D”话题SMX/Scan; 14 | - sport_control包读取joystick输入和其他指令，使用unitree_sdk2提供的接口控制机器狗; 15 | - 使用SLAM Toolbox建图时额外ros2 launch sport_control slam_launch.py; 16 | - 使用Nav2导航时额外ros2 launch sport_control nav_launch.py; 17 | - 使用Amov机架跟踪时额外ros2 launch sport_control g1_launch.py; 18 | - SLAM Toolbox目前是纯里程计建图，请擅长SLAM的同志自行把地图匹配加进去; 19 | - Nav2同样请自行调整，加载的地图记得改成自己的; 20 | - 也适用于Ubuntu20.04 foxy系统，把apt install的软件改为-foxy-即可; 21 | 22 | ## 🎥 视频演示 23 | ### 最新进展(点击图片进入视频) 24 | 纯里程计站立/四足切换建图效果 25 | [![主演示视频](https://i1.hdslb.com/bfs/archive/4f60453cb37ce5e4f593f03084dbecd0fdddc27e.jpg)](https://www.bilibili.com/video/BV1UtQfYJExu) 26 | 27 | #### 实验记录 28 | 1. 站立行走误差1%，四足行走误差0.5% 29 | [![实验1](https://i1.hdslb.com/bfs/archive/10e501bc7a93c77c1c3f41f163526b630b0afa3f.jpg)](https://www.bilibili.com/video/BV18Q9JYEEdn/) 30 | 31 | 2. 爬楼梯高度误差小于5cm 32 | [![实验2](https://i0.hdslb.com/bfs/archive/c469a3dd37522f6b7dcdbdbb2c135be599eefa7b.jpg)](https://www.bilibili.com/video/BV1VV9ZYZEcH/) 33 | 34 | 3. 长距离测试，受磁场变化影响，380米运动偏差3.3% 35 | [![实验3](https://i0.hdslb.com/bfs/archive/481731d2db755bbe087f44aeb3f48db29c159ada.jpg)](https://www.bilibili.com/video/BV1BhRAYDEsV/) 36 | 37 | 4. 语音控制机器狗，实现意图猜测和在预建地图导航。比如说“没有纸张了”，自动执行导航‘去仓库’ 38 | [![实验4](https://i2.hdslb.com/bfs/archive/5b95c6eda3b6c9c8e0ba4124c1af9f3da10f39d2.jpg)](https://www.bilibili.com/video/BV1HCQBYUEvk/) 39 | - 额外安装https://github.com/ShineMinxing/Ros2Chat 40 | 41 | 5. 机器狗与吊舱的协同光点/人脸跟踪 42 | [![实验5](https://i0.hdslb.com/bfs/archive/5496e9d0b40915c62b69701fd1e23af7d6ffe7de.jpg)](https://www.bilibili.com/video/BV1faG1z3EFF/) 43 | - 额外安装https://github.com/ShineMinxing/Ros2ImageProcess.git，https://github.com/ShineMinxing/Ros2AmovG1.git 44 | 45 | ## ⚙️ 安装指南 46 | 47 | - Use Ubuntu 22.04, ROS2 Humble 48 | ```bash 49 | sudo apt install ros-humble-joy ros-humble-nav2-msgs ros-humble-slam-toolbox ros-humble-nav2-bringup python3-pip libopencv-dev ros-humble-cv-bridge ros-humble-image-transport ros-humble-compressed-image-transport 50 | mkdir -p ~/ros2_ws/LeggedRobot/src && cd ~/ros2_ws/LeggedRobot/src 51 | git clone --recursive https://github.com/ShineMinxing/Ros2Go2Estimator.git 52 | cd .. 53 | # 1. 搜索工程中的所有 /home/unitree/ros2_ws/LeggedRobot，替换为您的路径 54 | # 2. 把 src/Ros2Go2Estimator/config.yaml 中的所有 br0 替换为您的网卡名，如 enxf8e43b808e06 55 | colcon build 56 | ros2 launch sport_control go2_launch.py 57 | ``` 58 | - 同时按下手柄的LT、RT，解锁/锁定手柄；按住RT+左摇杆进行移动；按住RT+右摇杆进行旋转；更多操作请看sport_control_node.cpp。 59 | 60 | ## 📄 相关文档 61 | - 核心算法原理: [技术文档](https://www.notion.so/Ros2Go2-1e3a3ea29e778044a4c9c35df4c27b22) 62 | - 历史项目参考: [Aliengo ROS1项目](https://github.com/ShineMinxing/FusionEstimation.git) 63 | 64 | ## 📧 联系我们 65 | ``` 66 | 博士团队: 401435318@qq.com 67 | 研究所: 中国科学院光电技术研究所 68 | ``` 69 | 70 | > 📌 注意：当前为开发预览版，完整文档正在编写中 71 | `` -------------------------------------------------------------------------------- /config.yaml: -------------------------------------------------------------------------------- 1 | dds_rostopic_node: 2 | ros__parameters: 3 | # 网络接口 4 | network_interface: "wlp0s20f3" 5 | 6 | # ROS2 发布的话题名 7 | pub_imu_topic: "SMX/Go2IMU" 8 | pub_joint_topic: "SMX/Go2Joint" 9 | pub_cloud_topic: "SMX/Go2Lidar" 10 | 11 | # DDS 订阅的原始 topic 12 | dds_lowstate_topic: "rt/lowstate" 13 | dds_pointcloud_topic: "rt/utlidar/cloud" 14 | 15 | # GStreamer 拉流管线 16 | gst_pipeline: > 17 | udpsrc address=230.1.1.1 port=1720 multicast-iface=wlp0s20f3 ! 18 | application/x-rtp, media=video, encoding-name=H264 ! 19 | rtph264depay ! h264parse ! avdec_h264 ! 20 | videoconvert ! video/x-raw,width=1280,height=720,format=BGR ! 21 | appsink drop=1 sync=false 22 | pub_camera_topic: "SMX/Go2Camera" 23 | pub_camera_raw_enable: false 24 | pub_camera_compressed_enable: false 25 | 26 | fusion_estimator_node: 27 | ros__parameters: 28 | 29 | # 订阅Go2传感器Topic 30 | sub_imu_topic: "SMX/Go2IMU" 31 | sub_joint_topic: "SMX/Go2Joint" 32 | 33 | # 订阅复位/模式命令的 ROS2 Topic 34 | sub_mode_topic: "SMX/JoyStringCmd" 35 | 36 | # 发布融合结果的 Topic 37 | pub_estimation_topic: "SMX/Estimation" 38 | 39 | # 发布全 3D 里程计的 Topic 40 | pub_odom_topic: "SMX/Odom" 41 | 42 | # 发布平面里程计的 Topic 43 | pub_odom2d_topic: "SMX/Odom_2D" 44 | 45 | # Odometry 消息里的 frame_id 46 | odom_frame: "odom" 47 | 48 | # Odometry 消息里的 child_frame_id 49 | base_frame: "base_link" 50 | base_frame_2d: "base_link_2D" 51 | 52 | # URDF 文件路径（相对于 package 根目录） 53 | urdf_file: "cfg/go2_description.urdf" 54 | 55 | # 可在线调整的三轴补偿角，单位：度 56 | Modify_Par_1: 0.0 57 | Modify_Par_2: 0.0 58 | Modify_Par_3: 0.0 59 | 60 | message_handle_node: 61 | ros__parameters: 62 | # 订阅的 PointCloud2 话题 63 | sub_pointcloud_topic: "/SMX/Go2Lidar" 64 | # 发布的 LaserScan 话题 65 | pub_laserscan_topic: "/SMX/Scan" 66 | 67 | # 订阅的 3D 里程计话题（用于发布 odom→base_link TF） 68 | sub_odom_topic: "/SMX/Odom" 69 | # 订阅的 2D 里程计话题（用于发布 odom→base_link_2D TF） 70 | sub_odom2d_topic: "/SMX/Odom_2D" 71 | 72 | # LaserScan 参数 73 | angle_min: -1.5708 # -π/2 74 | angle_max: 1.5708 # π/2 75 | angle_increment: 0.0056 76 | range_min: 0.5 77 | range_max: 30.0 78 | min_height: 0.1 79 | max_height: 10.0 80 | 81 | # TF frames 82 | map_frame: "map" 83 | odom_frame: "odom" 84 | base_frame: "base_link" 85 | base_frame_2d: "base_link_2D" 86 | utlidar_frame: "utlidar_lidar" 87 | 88 | # 静态 TF 从 base_link 到 utlidar_lidar 的 translation + RPY 89 | static_tx: 0.28945 90 | static_ty: 0.0 91 | static_tz: -0.046825 92 | static_r: 0.0 # roll 93 | static_p: 2.8782 # pitch 94 | static_y: 0.0 # yaw 95 | 96 | sport_control_node: 97 | ros__parameters: 98 | # 网络接口 99 | network_interface: "wlp0s20f3" 100 | 101 | # 各个 ROS 话题名称 102 | joy_topic: "/joy" 103 | sport_cmd_topic: "SMX/SportCmd" 104 | guide_topic: "/cmd_vel" 105 | joy_string_cmd: "SMX/JoyStringCmd" 106 | joy_float_cmd: "SMX/JoyFloatCmd" 107 | 108 | # 导航目标 pose 的 frame_id（sendGoal 中用） 109 | map_frame_id: "map" -------------------------------------------------------------------------------- /dds_rostopic/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(dds_rostopic) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(sensor_msgs REQUIRED) 8 | find_package(unitree_sdk2 REQUIRED) 9 | find_package(OpenCV REQUIRED) 10 | find_package(cv_bridge REQUIRED) 11 | find_package(PkgConfig REQUIRED) 12 | find_package(std_msgs REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(tf2 REQUIRED) 15 | find_package(tf2_ros REQUIRED) 16 | find_package(tf2_geometry_msgs REQUIRED) 17 | find_package(image_transport REQUIRED) 18 | find_package(compressed_image_transport REQUIRED) 19 | 20 | pkg_check_modules(GSTREAMER REQUIRED gstreamer-1.0) 21 | include_directories(${GSTREAMER_INCLUDE_DIRS}) 22 | link_directories(${GSTREAMER_LIBRARY_DIRS}) 23 | 24 | add_executable(dds_rostopic_node src/dds_rostopic_node.cpp) 25 | 26 | target_include_directories(dds_rostopic_node 27 | PRIVATE 28 | ${rclcpp_INCLUDE_DIRS} 29 | ${sensor_msgs_INCLUDE_DIRS} 30 | ${unitree_sdk2_INCLUDE_DIRS} 31 | ) 32 | 33 | target_link_libraries(dds_rostopic_node 34 | ${ament_LIBRARIES} 35 | unitree_sdk2 36 | ddsc ddscxx 37 | tf2_ros::tf2_ros 38 | ${OpenCV_LIBRARIES} 39 | ) 40 | 41 | ament_target_dependencies(dds_rostopic_node 42 | rclcpp 43 | sensor_msgs 44 | tf2_geometry_msgs 45 | unitree_sdk2 46 | OpenCV 47 | cv_bridge 48 | image_transport 49 | compressed_image_transport 50 | ) 51 | 52 | install(TARGETS 53 | dds_rostopic_node 54 | DESTINATION lib/${PROJECT_NAME} 55 | ) 56 | 57 | ament_package() -------------------------------------------------------------------------------- /dds_rostopic/package.xml: -------------------------------------------------------------------------------- 1 | 2 | dds_rostopic 3 | 0.0.0 4 | TODO: Package description 5 | 6 | shine 7 | TODO: License declaration 8 | 9 | ament_cmake 10 | rclcpp 11 | sensor_msgs 12 | unitree_sdk2 13 | opencv4 14 | cv_bridge 15 | std_msgs 16 | geometry_msgs 17 | tf2 18 | tf2_ros 19 | tf2_geometry_msgs 20 | image_transport 21 | compressed_image_transport 22 | gstreamer 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /fusion_estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | set(CMAKE_CXX_STANDARD 17) 3 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 4 | 5 | project(fusion_estimator) 6 | 7 | # 查找依赖 8 | find_package(ament_cmake REQUIRED) 9 | find_package(rclcpp REQUIRED) 10 | find_package(sensor_msgs REQUIRED) 11 | find_package(rosidl_default_generators REQUIRED) 12 | find_package(rosidl_cmake REQUIRED) 13 | find_package(builtin_interfaces REQUIRED) 14 | find_package(urdfdom_headers REQUIRED) 15 | find_package(urdfdom REQUIRED) 16 | find_package(nav_msgs REQUIRED) 17 | find_package(tf2 REQUIRED) 18 | find_package(tf2_geometry_msgs REQUIRED) 19 | 20 | # —— 后备实现 rosidl_get_typesupport_target —— 21 | if(NOT COMMAND rosidl_get_typesupport_target) 22 | function(rosidl_get_typesupport_target var generate_interfaces_target typesupport_name) 23 | # 检查基础目标是否存在 24 | if(NOT TARGET ${generate_interfaces_target}) 25 | message(FATAL_ERROR 26 | "${generate_interfaces_target} is not a CMake target. " 27 | "Did you run rosidl_generate_interfaces() on the correct target?") 28 | endif() 29 | # typesupport 目标名就是 __ 30 | set(output_target "${generate_interfaces_target}__${typesupport_name}") 31 | if(NOT TARGET ${output_target}) 32 | message(FATAL_ERROR 33 | "Cannot find typesupport target: ${output_target}") 34 | endif() 35 | # 返回给调用者 36 | set(${var} "${output_target}" PARENT_SCOPE) 37 | endfunction() 38 | endif() 39 | 40 | # 生成消息接口 41 | rosidl_generate_interfaces(${PROJECT_NAME} 42 | "msg/FusionEstimatorTest.msg" 43 | DEPENDENCIES builtin_interfaces 44 | ) 45 | 46 | # 获取 src/ 下的所有 .cpp 和 .c 源文件 47 | file(GLOB_RECURSE ESTIMATOR_SOURCES 48 | "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp" 49 | "${CMAKE_CURRENT_SOURCE_DIR}/src/*.c" 50 | ) 51 | 52 | # 获取 C++ 类型支持目标 53 | rosidl_get_typesupport_target( 54 | cpp_typesupport_target 55 | ${PROJECT_NAME} 56 | "rosidl_typesupport_cpp" 57 | ) 58 | 59 | # 创建可执行文件 60 | add_executable(fusion_estimator_node ${ESTIMATOR_SOURCES}) 61 | 62 | # 包含目录 63 | target_include_directories(fusion_estimator_node PRIVATE 64 | ${rclcpp_INCLUDE_DIRS} 65 | ${sensor_msgs_INCLUDE_DIRS} 66 | ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp 67 | ${CMAKE_CURRENT_SOURCE_DIR}/src 68 | ${urdfdom_headers_INCLUDE_DIRS} 69 | ${urdfdom_INCLUDE_DIRS} 70 | ) 71 | 72 | # 声明依赖 73 | ament_target_dependencies(fusion_estimator_node 74 | rclcpp 75 | sensor_msgs 76 | nav_msgs 77 | tf2 78 | tf2_geometry_msgs 79 | ) 80 | 81 | # 链接库 82 | target_link_libraries(fusion_estimator_node 83 | ${ament_LIBRARIES} 84 | ${cpp_typesupport_target} 85 | ${urdfdom_LIBRARIES} 86 | ) 87 | 88 | # 安装目标 89 | install(TARGETS fusion_estimator_node 90 | DESTINATION lib/${PROJECT_NAME} 91 | ) 92 | 93 | # 导出依赖 94 | ament_export_dependencies(rosidl_default_runtime builtin_interfaces) 95 | ament_package() 96 | -------------------------------------------------------------------------------- /fusion_estimator/msg/FusionEstimatorTest.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time stamp 2 | float64[20] others 3 | float64[12] feet_based_position 4 | float64[12] feet_based_velocity 5 | float64[9] estimated_xyz 6 | float64[9] estimated_rpy 7 | float64[12] data_check_a 8 | float64[12] data_check_b 9 | float64[12] data_check_c 10 | float64[12] data_check_d 11 | float64[12] data_check_e -------------------------------------------------------------------------------- /fusion_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fusion_estimator 5 | 0.0.0 6 | sensor signal fusion estimation package 7 | Sun Minxing 8 | Apache-2.0 9 | 10 | ament_cmake 11 | rosidl_default_runtime 12 | rosidl_interface_packages 13 | 14 | rclcpp 15 | sensor_msgs 16 | unitree_sdk2 17 | builtin_interfaces 18 | urdfdom 19 | urdfdom_parser 20 | rosidl_cmake 21 | rosidl_default_generators 22 | 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/.gitignore: -------------------------------------------------------------------------------- 1 | fusion_estimator_node.cpp 2 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "Estimator"] 2 | path = Estimator 3 | url = https://github.com/ShineMinxing/SMXFusionEstimator.git 4 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/Dense: -------------------------------------------------------------------------------- 1 | #include "Core" 2 | #include "LU" 3 | #include "Cholesky" 4 | #include "QR" 5 | #include "SVD" 6 | #include "Geometry" 7 | #include "Eigenvalues" 8 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/Eigen: -------------------------------------------------------------------------------- 1 | #include "Dense" 2 | #include "Sparse" 3 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Core/Inverse.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014-2019 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_INVERSE_H 11 | #define EIGEN_INVERSE_H 12 | 13 | namespace Eigen { 14 | 15 | template class InverseImpl; 16 | 17 | namespace internal { 18 | 19 | template 20 | struct traits > 21 | : traits 22 | { 23 | typedef typename XprType::PlainObject PlainObject; 24 | typedef traits BaseTraits; 25 | enum { 26 | Flags = BaseTraits::Flags & RowMajorBit 27 | }; 28 | }; 29 | 30 | } // end namespace internal 31 | 32 | /** \class Inverse 33 | * 34 | * \brief Expression of the inverse of another expression 35 | * 36 | * \tparam XprType the type of the expression we are taking the inverse 37 | * 38 | * This class represents an abstract expression of A.inverse() 39 | * and most of the time this is the only way it is used. 40 | * 41 | */ 42 | template 43 | class Inverse : public InverseImpl::StorageKind> 44 | { 45 | public: 46 | typedef typename XprType::StorageIndex StorageIndex; 47 | typedef typename XprType::Scalar Scalar; 48 | typedef typename internal::ref_selector::type XprTypeNested; 49 | typedef typename internal::remove_all::type XprTypeNestedCleaned; 50 | typedef typename internal::ref_selector::type Nested; 51 | typedef typename internal::remove_all::type NestedExpression; 52 | 53 | explicit EIGEN_DEVICE_FUNC Inverse(const XprType &xpr) 54 | : m_xpr(xpr) 55 | {} 56 | 57 | EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.cols(); } 58 | EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.rows(); } 59 | 60 | EIGEN_DEVICE_FUNC const XprTypeNestedCleaned& nestedExpression() const { return m_xpr; } 61 | 62 | protected: 63 | XprTypeNested m_xpr; 64 | }; 65 | 66 | // Generic API dispatcher 67 | template 68 | class InverseImpl 69 | : public internal::generic_xpr_base >::type 70 | { 71 | public: 72 | typedef typename internal::generic_xpr_base >::type Base; 73 | typedef typename XprType::Scalar Scalar; 74 | private: 75 | 76 | Scalar coeff(Index row, Index col) const; 77 | Scalar coeff(Index i) const; 78 | }; 79 | 80 | namespace internal { 81 | 82 | /** \internal 83 | * \brief Default evaluator for Inverse expression. 84 | * 85 | * This default evaluator for Inverse expression simply evaluate the inverse into a temporary 86 | * by a call to internal::call_assignment_no_alias. 87 | * Therefore, inverse implementers only have to specialize Assignment, ...> for 88 | * there own nested expression. 89 | * 90 | * \sa class Inverse 91 | */ 92 | template 93 | struct unary_evaluator > 94 | : public evaluator::PlainObject> 95 | { 96 | typedef Inverse InverseType; 97 | typedef typename InverseType::PlainObject PlainObject; 98 | typedef evaluator Base; 99 | 100 | enum { Flags = Base::Flags | EvalBeforeNestingBit }; 101 | 102 | unary_evaluator(const InverseType& inv_xpr) 103 | : m_result(inv_xpr.rows(), inv_xpr.cols()) 104 | { 105 | ::new (static_cast(this)) Base(m_result); 106 | internal::call_assignment_no_alias(m_result, inv_xpr); 107 | } 108 | 109 | protected: 110 | PlainObject m_result; 111 | }; 112 | 113 | } // end namespace internal 114 | 115 | } // end namespace Eigen 116 | 117 | #endif // EIGEN_INVERSE_H 118 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Core/NoAlias.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 | // 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_NOALIAS_H 11 | #define EIGEN_NOALIAS_H 12 | 13 | namespace Eigen { 14 | 15 | /** \class NoAlias 16 | * \ingroup Core_Module 17 | * 18 | * \brief Pseudo expression providing an operator = assuming no aliasing 19 | * 20 | * \tparam ExpressionType the type of the object on which to do the lazy assignment 21 | * 22 | * This class represents an expression with special assignment operators 23 | * assuming no aliasing between the target expression and the source expression. 24 | * More precisely it alloas to bypass the EvalBeforeAssignBit flag of the source expression. 25 | * It is the return type of MatrixBase::noalias() 26 | * and most of the time this is the only way it is used. 27 | * 28 | * \sa MatrixBase::noalias() 29 | */ 30 | template class StorageBase> 31 | class NoAlias 32 | { 33 | public: 34 | typedef typename ExpressionType::Scalar Scalar; 35 | 36 | EIGEN_DEVICE_FUNC 37 | explicit NoAlias(ExpressionType& expression) : m_expression(expression) {} 38 | 39 | template 40 | EIGEN_DEVICE_FUNC 41 | EIGEN_STRONG_INLINE ExpressionType& operator=(const StorageBase& other) 42 | { 43 | call_assignment_no_alias(m_expression, other.derived(), internal::assign_op()); 44 | return m_expression; 45 | } 46 | 47 | template 48 | EIGEN_DEVICE_FUNC 49 | EIGEN_STRONG_INLINE ExpressionType& operator+=(const StorageBase& other) 50 | { 51 | call_assignment_no_alias(m_expression, other.derived(), internal::add_assign_op()); 52 | return m_expression; 53 | } 54 | 55 | template 56 | EIGEN_DEVICE_FUNC 57 | EIGEN_STRONG_INLINE ExpressionType& operator-=(const StorageBase& other) 58 | { 59 | call_assignment_no_alias(m_expression, other.derived(), internal::sub_assign_op()); 60 | return m_expression; 61 | } 62 | 63 | EIGEN_DEVICE_FUNC 64 | ExpressionType& expression() const 65 | { 66 | return m_expression; 67 | } 68 | 69 | protected: 70 | ExpressionType& m_expression; 71 | }; 72 | 73 | /** \returns a pseudo expression of \c *this with an operator= assuming 74 | * no aliasing between \c *this and the source expression. 75 | * 76 | * More precisely, noalias() allows to bypass the EvalBeforeAssignBit flag. 77 | * Currently, even though several expressions may alias, only product 78 | * expressions have this flag. Therefore, noalias() is only useful when 79 | * the source expression contains a matrix product. 80 | * 81 | * Here are some examples where noalias is useful: 82 | * \code 83 | * D.noalias() = A * B; 84 | * D.noalias() += A.transpose() * B; 85 | * D.noalias() -= 2 * A * B.adjoint(); 86 | * \endcode 87 | * 88 | * On the other hand the following example will lead to a \b wrong result: 89 | * \code 90 | * A.noalias() = A * B; 91 | * \endcode 92 | * because the result matrix A is also an operand of the matrix product. Therefore, 93 | * there is no alternative than evaluating A * B in a temporary, that is the default 94 | * behavior when you write: 95 | * \code 96 | * A = A * B; 97 | * \endcode 98 | * 99 | * \sa class NoAlias 100 | */ 101 | template 102 | NoAlias EIGEN_DEVICE_FUNC MatrixBase::noalias() 103 | { 104 | return NoAlias(derived()); 105 | } 106 | 107 | } // end namespace Eigen 108 | 109 | #endif // EIGEN_NOALIAS_H 110 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Core/VectorBlock.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 | #ifndef EIGEN_VECTORBLOCK_H 12 | #define EIGEN_VECTORBLOCK_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | template 18 | struct traits > 19 | : public traits::Flags & RowMajorBit ? 1 : Size, 21 | traits::Flags & RowMajorBit ? Size : 1> > 22 | { 23 | }; 24 | } 25 | 26 | /** \class VectorBlock 27 | * \ingroup Core_Module 28 | * 29 | * \brief Expression of a fixed-size or dynamic-size sub-vector 30 | * 31 | * \tparam VectorType the type of the object in which we are taking a sub-vector 32 | * \tparam Size size of the sub-vector we are taking at compile time (optional) 33 | * 34 | * This class represents an expression of either a fixed-size or dynamic-size sub-vector. 35 | * It is the return type of DenseBase::segment(Index,Index) and DenseBase::segment(Index) and 36 | * most of the time this is the only way it is used. 37 | * 38 | * However, if you want to directly manipulate sub-vector expressions, 39 | * for instance if you want to write a function returning such an expression, you 40 | * will need to use this class. 41 | * 42 | * Here is an example illustrating the dynamic case: 43 | * \include class_VectorBlock.cpp 44 | * Output: \verbinclude class_VectorBlock.out 45 | * 46 | * \note Even though this expression has dynamic size, in the case where \a VectorType 47 | * has fixed size, this expression inherits a fixed maximal size which means that evaluating 48 | * it does not cause a dynamic memory allocation. 49 | * 50 | * Here is an example illustrating the fixed-size case: 51 | * \include class_FixedVectorBlock.cpp 52 | * Output: \verbinclude class_FixedVectorBlock.out 53 | * 54 | * \sa class Block, DenseBase::segment(Index,Index,Index,Index), DenseBase::segment(Index,Index) 55 | */ 56 | template class VectorBlock 57 | : public Block::Flags & RowMajorBit ? 1 : Size, 59 | internal::traits::Flags & RowMajorBit ? Size : 1> 60 | { 61 | typedef Block::Flags & RowMajorBit ? 1 : Size, 63 | internal::traits::Flags & RowMajorBit ? Size : 1> Base; 64 | enum { 65 | IsColVector = !(internal::traits::Flags & RowMajorBit) 66 | }; 67 | public: 68 | EIGEN_DENSE_PUBLIC_INTERFACE(VectorBlock) 69 | 70 | using Base::operator=; 71 | 72 | /** Dynamic-size constructor 73 | */ 74 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 75 | VectorBlock(VectorType& vector, Index start, Index size) 76 | : Base(vector, 77 | IsColVector ? start : 0, IsColVector ? 0 : start, 78 | IsColVector ? size : 1, IsColVector ? 1 : size) 79 | { 80 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorBlock); 81 | } 82 | 83 | /** Fixed-size constructor 84 | */ 85 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 86 | VectorBlock(VectorType& vector, Index start) 87 | : Base(vector, IsColVector ? start : 0, IsColVector ? 0 : start) 88 | { 89 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorBlock); 90 | } 91 | }; 92 | 93 | 94 | } // end namespace Eigen 95 | 96 | #endif // EIGEN_VECTORBLOCK_H 97 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2019 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_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H 11 | #define EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H 12 | 13 | namespace Eigen { 14 | namespace internal { 15 | 16 | // Forward declarations of the generic math functions 17 | // implemented in GenericPacketMathFunctions.h 18 | // This is needed to workaround a circular dependency. 19 | 20 | /*************************************************************************** 21 | * Some generic implementations to be used by implementors 22 | ***************************************************************************/ 23 | 24 | /** Default implementation of pfrexp. 25 | * It is expected to be called by implementers of template<> pfrexp. 26 | */ 27 | template EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC 28 | Packet pfrexp_generic(const Packet& a, Packet& exponent); 29 | 30 | // Extracts the biased exponent value from Packet p, and casts the results to 31 | // a floating-point Packet type. Used by pfrexp_generic. Override this if 32 | // there is no unpacket_traits::integer_packet. 33 | template EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC 34 | Packet pfrexp_generic_get_biased_exponent(const Packet& p); 35 | 36 | /** Default implementation of pldexp. 37 | * It is expected to be called by implementers of template<> pldexp. 38 | */ 39 | template EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC 40 | Packet pldexp_generic(const Packet& a, const Packet& exponent); 41 | 42 | /** \internal \returns log(x) for single precision float */ 43 | template 44 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 45 | EIGEN_UNUSED 46 | Packet plog_float(const Packet _x); 47 | 48 | /** \internal \returns log2(x) for single precision float */ 49 | template 50 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 51 | EIGEN_UNUSED 52 | Packet plog2_float(const Packet _x); 53 | 54 | /** \internal \returns log(x) for single precision float */ 55 | template 56 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 57 | EIGEN_UNUSED 58 | Packet plog_double(const Packet _x); 59 | 60 | /** \internal \returns log2(x) for single precision float */ 61 | template 62 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 63 | EIGEN_UNUSED 64 | Packet plog2_double(const Packet _x); 65 | 66 | /** \internal \returns log(1 + x) */ 67 | template 68 | Packet generic_plog1p(const Packet& x); 69 | 70 | /** \internal \returns exp(x)-1 */ 71 | template 72 | Packet generic_expm1(const Packet& x); 73 | 74 | /** \internal \returns exp(x) for single precision float */ 75 | template 76 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 77 | EIGEN_UNUSED 78 | Packet pexp_float(const Packet _x); 79 | 80 | /** \internal \returns exp(x) for double precision real numbers */ 81 | template 82 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 83 | EIGEN_UNUSED 84 | Packet pexp_double(const Packet _x); 85 | 86 | /** \internal \returns sin(x) for single precision float */ 87 | template 88 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 89 | EIGEN_UNUSED 90 | Packet psin_float(const Packet& x); 91 | 92 | /** \internal \returns cos(x) for single precision float */ 93 | template 94 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 95 | EIGEN_UNUSED 96 | Packet pcos_float(const Packet& x); 97 | 98 | /** \internal \returns sqrt(x) for complex types */ 99 | template 100 | EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS 101 | EIGEN_UNUSED 102 | Packet psqrt_complex(const Packet& a); 103 | 104 | template struct ppolevl; 105 | 106 | 107 | } // end namespace internal 108 | } // end namespace Eigen 109 | 110 | #endif // EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H 111 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Core/arch/Default/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 | // Copyright (C) 2019 Rasmus Munk Larsen 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_GENERIC_TYPE_CASTING_H 12 | #define EIGEN_GENERIC_TYPE_CASTING_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | template<> 19 | struct scalar_cast_op { 20 | EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op) 21 | typedef Eigen::half result_type; 22 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::half operator() (const float& a) const { 23 | #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \ 24 | (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) 25 | return __float2half(a); 26 | #else 27 | return Eigen::half(a); 28 | #endif 29 | } 30 | }; 31 | 32 | template<> 33 | struct functor_traits > 34 | { enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; 35 | 36 | 37 | template<> 38 | struct scalar_cast_op { 39 | EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op) 40 | typedef Eigen::half result_type; 41 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::half operator() (const int& a) const { 42 | #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \ 43 | (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) 44 | return __float2half(static_cast(a)); 45 | #else 46 | return Eigen::half(static_cast(a)); 47 | #endif 48 | } 49 | }; 50 | 51 | template<> 52 | struct functor_traits > 53 | { enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; 54 | 55 | 56 | template<> 57 | struct scalar_cast_op { 58 | EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op) 59 | typedef float result_type; 60 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float operator() (const Eigen::half& a) const { 61 | #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \ 62 | (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) 63 | return __half2float(a); 64 | #else 65 | return static_cast(a); 66 | #endif 67 | } 68 | }; 69 | 70 | template<> 71 | struct functor_traits > 72 | { enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; 73 | 74 | 75 | template<> 76 | struct scalar_cast_op { 77 | EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op) 78 | typedef Eigen::bfloat16 result_type; 79 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::bfloat16 operator() (const float& a) const { 80 | return Eigen::bfloat16(a); 81 | } 82 | }; 83 | 84 | template<> 85 | struct functor_traits > 86 | { enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; 87 | 88 | 89 | template<> 90 | struct scalar_cast_op { 91 | EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op) 92 | typedef Eigen::bfloat16 result_type; 93 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::bfloat16 operator() (const int& a) const { 94 | return Eigen::bfloat16(static_cast(a)); 95 | } 96 | }; 97 | 98 | template<> 99 | struct functor_traits > 100 | { enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; 101 | 102 | 103 | template<> 104 | struct scalar_cast_op { 105 | EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op) 106 | typedef float result_type; 107 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float operator() (const Eigen::bfloat16& a) const { 108 | return static_cast(a); 109 | } 110 | }; 111 | 112 | template<> 113 | struct functor_traits > 114 | { enum { Cost = NumTraits::AddCost, PacketAccess = false }; }; 115 | 116 | 117 | } 118 | } 119 | 120 | #endif // EIGEN_GENERIC_TYPE_CASTING_H 121 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Core/arch/GPU/MathFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 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_MATH_FUNCTIONS_GPU_H 11 | #define EIGEN_MATH_FUNCTIONS_GPU_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // Make sure this is only available when targeting a GPU: we don't want to 18 | // introduce conflicts between these packet_traits definitions and the ones 19 | // we'll use on the host side (SSE, AVX, ...) 20 | #if defined(EIGEN_GPUCC) && defined(EIGEN_USE_GPU) 21 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 22 | float4 plog(const float4& a) 23 | { 24 | return make_float4(logf(a.x), logf(a.y), logf(a.z), logf(a.w)); 25 | } 26 | 27 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 28 | double2 plog(const double2& a) 29 | { 30 | using ::log; 31 | return make_double2(log(a.x), log(a.y)); 32 | } 33 | 34 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 35 | float4 plog1p(const float4& a) 36 | { 37 | return make_float4(log1pf(a.x), log1pf(a.y), log1pf(a.z), log1pf(a.w)); 38 | } 39 | 40 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 41 | double2 plog1p(const double2& a) 42 | { 43 | return make_double2(log1p(a.x), log1p(a.y)); 44 | } 45 | 46 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 47 | float4 pexp(const float4& a) 48 | { 49 | return make_float4(expf(a.x), expf(a.y), expf(a.z), expf(a.w)); 50 | } 51 | 52 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 53 | double2 pexp(const double2& a) 54 | { 55 | using ::exp; 56 | return make_double2(exp(a.x), exp(a.y)); 57 | } 58 | 59 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 60 | float4 pexpm1(const float4& a) 61 | { 62 | return make_float4(expm1f(a.x), expm1f(a.y), expm1f(a.z), expm1f(a.w)); 63 | } 64 | 65 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 66 | double2 pexpm1(const double2& a) 67 | { 68 | return make_double2(expm1(a.x), expm1(a.y)); 69 | } 70 | 71 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 72 | float4 psqrt(const float4& a) 73 | { 74 | return make_float4(sqrtf(a.x), sqrtf(a.y), sqrtf(a.z), sqrtf(a.w)); 75 | } 76 | 77 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 78 | double2 psqrt(const double2& a) 79 | { 80 | using ::sqrt; 81 | return make_double2(sqrt(a.x), sqrt(a.y)); 82 | } 83 | 84 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 85 | float4 prsqrt(const float4& a) 86 | { 87 | return make_float4(rsqrtf(a.x), rsqrtf(a.y), rsqrtf(a.z), rsqrtf(a.w)); 88 | } 89 | 90 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 91 | double2 prsqrt(const double2& a) 92 | { 93 | return make_double2(rsqrt(a.x), rsqrt(a.y)); 94 | } 95 | 96 | 97 | #endif 98 | 99 | } // end namespace internal 100 | 101 | } // end namespace Eigen 102 | 103 | #endif // EIGEN_MATH_FUNCTIONS_GPU_H 104 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Core/arch/NEON/MathFunctions.h: -------------------------------------------------------------------------------- 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_MATH_FUNCTIONS_NEON_H 9 | #define EIGEN_MATH_FUNCTIONS_NEON_H 10 | 11 | namespace Eigen { 12 | 13 | namespace internal { 14 | 15 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f pexp(const Packet2f& x) 16 | { return pexp_float(x); } 17 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f pexp(const Packet4f& x) 18 | { return pexp_float(x); } 19 | 20 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f plog(const Packet2f& x) 21 | { return plog_float(x); } 22 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f plog(const Packet4f& x) 23 | { return plog_float(x); } 24 | 25 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f psin(const Packet2f& x) 26 | { return psin_float(x); } 27 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f psin(const Packet4f& x) 28 | { return psin_float(x); } 29 | 30 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f pcos(const Packet2f& x) 31 | { return pcos_float(x); } 32 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f pcos(const Packet4f& x) 33 | { return pcos_float(x); } 34 | 35 | // Hyperbolic Tangent function. 36 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f ptanh(const Packet2f& x) 37 | { return internal::generic_fast_tanh_float(x); } 38 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f ptanh(const Packet4f& x) 39 | { return internal::generic_fast_tanh_float(x); } 40 | 41 | BF16_PACKET_FUNCTION(Packet4f, Packet4bf, psin) 42 | BF16_PACKET_FUNCTION(Packet4f, Packet4bf, pcos) 43 | BF16_PACKET_FUNCTION(Packet4f, Packet4bf, plog) 44 | BF16_PACKET_FUNCTION(Packet4f, Packet4bf, pexp) 45 | BF16_PACKET_FUNCTION(Packet4f, Packet4bf, ptanh) 46 | 47 | template <> 48 | EIGEN_STRONG_INLINE Packet4bf pfrexp(const Packet4bf& a, Packet4bf& exponent) { 49 | Packet4f fexponent; 50 | const Packet4bf out = F32ToBf16(pfrexp(Bf16ToF32(a), fexponent)); 51 | exponent = F32ToBf16(fexponent); 52 | return out; 53 | } 54 | 55 | template <> 56 | EIGEN_STRONG_INLINE Packet4bf pldexp(const Packet4bf& a, const Packet4bf& exponent) { 57 | return F32ToBf16(pldexp(Bf16ToF32(a), Bf16ToF32(exponent))); 58 | } 59 | 60 | //---------- double ---------- 61 | 62 | #if EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG 63 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d pexp(const Packet2d& x) 64 | { return pexp_double(x); } 65 | 66 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d plog(const Packet2d& x) 67 | { return plog_double(x); } 68 | 69 | #endif 70 | 71 | } // end namespace internal 72 | 73 | } // end namespace Eigen 74 | 75 | #endif // EIGEN_MATH_FUNCTIONS_NEON_H 76 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Core/arch/SSE/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_SSE_H 11 | #define EIGEN_TYPE_CASTING_SSE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | #ifndef EIGEN_VECTORIZE_AVX 18 | template <> 19 | struct type_casting_traits { 20 | enum { 21 | VectorizedCast = 1, 22 | SrcCoeffRatio = 1, 23 | TgtCoeffRatio = 1 24 | }; 25 | }; 26 | 27 | template <> 28 | struct type_casting_traits { 29 | enum { 30 | VectorizedCast = 1, 31 | SrcCoeffRatio = 1, 32 | TgtCoeffRatio = 1 33 | }; 34 | }; 35 | 36 | template <> 37 | struct type_casting_traits { 38 | enum { 39 | VectorizedCast = 1, 40 | SrcCoeffRatio = 2, 41 | TgtCoeffRatio = 1 42 | }; 43 | }; 44 | 45 | template <> 46 | struct type_casting_traits { 47 | enum { 48 | VectorizedCast = 1, 49 | SrcCoeffRatio = 1, 50 | TgtCoeffRatio = 2 51 | }; 52 | }; 53 | #endif 54 | 55 | template<> EIGEN_STRONG_INLINE Packet4i pcast(const Packet4f& a) { 56 | return _mm_cvttps_epi32(a); 57 | } 58 | 59 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet4i& a) { 60 | return _mm_cvtepi32_ps(a); 61 | } 62 | 63 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet2d& a, const Packet2d& b) { 64 | return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6)); 65 | } 66 | 67 | template<> EIGEN_STRONG_INLINE Packet2d pcast(const Packet4f& a) { 68 | // Simply discard the second half of the input 69 | return _mm_cvtps_pd(a); 70 | } 71 | 72 | template<> EIGEN_STRONG_INLINE Packet4i preinterpret(const Packet4f& a) { 73 | return _mm_castps_si128(a); 74 | } 75 | 76 | template<> EIGEN_STRONG_INLINE Packet4f preinterpret(const Packet4i& a) { 77 | return _mm_castsi128_ps(a); 78 | } 79 | 80 | template<> EIGEN_STRONG_INLINE Packet2d preinterpret(const Packet4i& a) { 81 | return _mm_castsi128_pd(a); 82 | } 83 | 84 | template<> EIGEN_STRONG_INLINE Packet4i preinterpret(const Packet2d& a) { 85 | return _mm_castpd_si128(a); 86 | } 87 | 88 | // Disable the following code since it's broken on too many platforms / compilers. 89 | //#elif defined(EIGEN_VECTORIZE_SSE) && (!EIGEN_ARCH_x86_64) && (!EIGEN_COMP_MSVC) 90 | #if 0 91 | 92 | template <> 93 | struct type_casting_traits { 94 | enum { 95 | VectorizedCast = 1, 96 | SrcCoeffRatio = 1, 97 | TgtCoeffRatio = 1 98 | }; 99 | }; 100 | 101 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet4h& a) { 102 | __int64_t a64 = _mm_cvtm64_si64(a.x); 103 | Eigen::half h = raw_uint16_to_half(static_cast(a64)); 104 | float f1 = static_cast(h); 105 | h = raw_uint16_to_half(static_cast(a64 >> 16)); 106 | float f2 = static_cast(h); 107 | h = raw_uint16_to_half(static_cast(a64 >> 32)); 108 | float f3 = static_cast(h); 109 | h = raw_uint16_to_half(static_cast(a64 >> 48)); 110 | float f4 = static_cast(h); 111 | return _mm_set_ps(f4, f3, f2, f1); 112 | } 113 | 114 | template <> 115 | struct type_casting_traits { 116 | enum { 117 | VectorizedCast = 1, 118 | SrcCoeffRatio = 1, 119 | TgtCoeffRatio = 1 120 | }; 121 | }; 122 | 123 | template<> EIGEN_STRONG_INLINE Packet4h pcast(const Packet4f& a) { 124 | EIGEN_ALIGN16 float aux[4]; 125 | pstore(aux, a); 126 | Eigen::half h0(aux[0]); 127 | Eigen::half h1(aux[1]); 128 | Eigen::half h2(aux[2]); 129 | Eigen::half h3(aux[3]); 130 | 131 | Packet4h result; 132 | result.x = _mm_set_pi16(h3.x, h2.x, h1.x, h0.x); 133 | return result; 134 | } 135 | 136 | #endif 137 | 138 | } // end namespace internal 139 | 140 | } // end namespace Eigen 141 | 142 | #endif // EIGEN_TYPE_CASTING_SSE_H 143 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Intel Corporation. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Intel Corporation nor the names of its contributors may 13 | be used to endorse or promote products derived from this software without 14 | specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 23 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | ******************************************************************************** 28 | * Content : Eigen bindings to LAPACKe 29 | * Real Schur needed to real unsymmetrical eigenvalues/eigenvectors. 30 | ******************************************************************************** 31 | */ 32 | 33 | #ifndef EIGEN_REAL_SCHUR_LAPACKE_H 34 | #define EIGEN_REAL_SCHUR_LAPACKE_H 35 | 36 | namespace Eigen { 37 | 38 | /** \internal Specialization for the data types supported by LAPACKe */ 39 | 40 | #define EIGEN_LAPACKE_SCHUR_REAL(EIGTYPE, LAPACKE_TYPE, LAPACKE_PREFIX, LAPACKE_PREFIX_U, EIGCOLROW, LAPACKE_COLROW) \ 41 | template<> template inline \ 42 | RealSchur >& \ 43 | RealSchur >::compute(const EigenBase& matrix, bool computeU) \ 44 | { \ 45 | eigen_assert(matrix.cols() == matrix.rows()); \ 46 | \ 47 | lapack_int n = internal::convert_index(matrix.cols()), sdim, info; \ 48 | lapack_int matrix_order = LAPACKE_COLROW; \ 49 | char jobvs, sort='N'; \ 50 | LAPACK_##LAPACKE_PREFIX_U##_SELECT2 select = 0; \ 51 | jobvs = (computeU) ? 'V' : 'N'; \ 52 | m_matU.resize(n, n); \ 53 | lapack_int ldvs = internal::convert_index(m_matU.outerStride()); \ 54 | m_matT = matrix; \ 55 | lapack_int lda = internal::convert_index(m_matT.outerStride()); \ 56 | Matrix wr, wi; \ 57 | wr.resize(n, 1); wi.resize(n, 1); \ 58 | info = LAPACKE_##LAPACKE_PREFIX##gees( matrix_order, jobvs, sort, select, n, (LAPACKE_TYPE*)m_matT.data(), lda, &sdim, (LAPACKE_TYPE*)wr.data(), (LAPACKE_TYPE*)wi.data(), (LAPACKE_TYPE*)m_matU.data(), ldvs ); \ 59 | if(info == 0) \ 60 | m_info = Success; \ 61 | else \ 62 | m_info = NoConvergence; \ 63 | \ 64 | m_isInitialized = true; \ 65 | m_matUisUptodate = computeU; \ 66 | return *this; \ 67 | \ 68 | } 69 | 70 | EIGEN_LAPACKE_SCHUR_REAL(double, double, d, D, ColMajor, LAPACK_COL_MAJOR) 71 | EIGEN_LAPACKE_SCHUR_REAL(float, float, s, S, ColMajor, LAPACK_COL_MAJOR) 72 | EIGEN_LAPACKE_SCHUR_REAL(double, double, d, D, RowMajor, LAPACK_ROW_MAJOR) 73 | EIGEN_LAPACKE_SCHUR_REAL(float, float, s, S, RowMajor, LAPACK_ROW_MAJOR) 74 | 75 | } // end namespace Eigen 76 | 77 | #endif // EIGEN_REAL_SCHUR_LAPACKE_H 78 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/Geometry/EulerAngles.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_EULERANGLES_H 11 | #define EIGEN_EULERANGLES_H 12 | 13 | namespace Eigen { 14 | 15 | /** \geometry_module \ingroup Geometry_Module 16 | * 17 | * 18 | * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2) 19 | * 20 | * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}. 21 | * For instance, in: 22 | * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode 23 | * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that 24 | * we have the following equality: 25 | * \code 26 | * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) 27 | * * AngleAxisf(ea[1], Vector3f::UnitX()) 28 | * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode 29 | * This corresponds to the right-multiply conventions (with right hand side frames). 30 | * 31 | * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi]. 32 | * 33 | * \sa class AngleAxis 34 | */ 35 | template 36 | EIGEN_DEVICE_FUNC inline Matrix::Scalar,3,1> 37 | MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const 38 | { 39 | EIGEN_USING_STD(atan2) 40 | EIGEN_USING_STD(sin) 41 | EIGEN_USING_STD(cos) 42 | /* Implemented from Graphics Gems IV */ 43 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) 44 | 45 | Matrix res; 46 | typedef Matrix Vector2; 47 | 48 | const Index odd = ((a0+1)%3 == a1) ? 0 : 1; 49 | const Index i = a0; 50 | const Index j = (a0 + 1 + odd)%3; 51 | const Index k = (a0 + 2 - odd)%3; 52 | 53 | if (a0==a2) 54 | { 55 | res[0] = atan2(coeff(j,i), coeff(k,i)); 56 | if((odd && res[0]Scalar(0))) 57 | { 58 | if(res[0] > Scalar(0)) { 59 | res[0] -= Scalar(EIGEN_PI); 60 | } 61 | else { 62 | res[0] += Scalar(EIGEN_PI); 63 | } 64 | Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); 65 | res[1] = -atan2(s2, coeff(i,i)); 66 | } 67 | else 68 | { 69 | Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); 70 | res[1] = atan2(s2, coeff(i,i)); 71 | } 72 | 73 | // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, 74 | // we can compute their respective rotation, and apply its inverse to M. Since the result must 75 | // be a rotation around x, we have: 76 | // 77 | // c2 s1.s2 c1.s2 1 0 0 78 | // 0 c1 -s1 * M = 0 c3 s3 79 | // -s2 s1.c2 c1.c2 0 -s3 c3 80 | // 81 | // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 82 | 83 | Scalar s1 = sin(res[0]); 84 | Scalar c1 = cos(res[0]); 85 | res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j)); 86 | } 87 | else 88 | { 89 | res[0] = atan2(coeff(j,k), coeff(k,k)); 90 | Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm(); 91 | if((odd && res[0]Scalar(0))) { 92 | if(res[0] > Scalar(0)) { 93 | res[0] -= Scalar(EIGEN_PI); 94 | } 95 | else { 96 | res[0] += Scalar(EIGEN_PI); 97 | } 98 | res[1] = atan2(-coeff(i,k), -c2); 99 | } 100 | else 101 | res[1] = atan2(-coeff(i,k), c2); 102 | Scalar s1 = sin(res[0]); 103 | Scalar c1 = cos(res[0]); 104 | res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j)); 105 | } 106 | if (!odd) 107 | res = -res; 108 | 109 | return res; 110 | } 111 | 112 | } // end namespace Eigen 113 | 114 | #endif // EIGEN_EULERANGLES_H 115 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/LU/Determinant.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_DETERMINANT_H 11 | #define EIGEN_DETERMINANT_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | template 18 | EIGEN_DEVICE_FUNC 19 | inline const typename Derived::Scalar bruteforce_det3_helper 20 | (const MatrixBase& matrix, int a, int b, int c) 21 | { 22 | return matrix.coeff(0,a) 23 | * (matrix.coeff(1,b) * matrix.coeff(2,c) - matrix.coeff(1,c) * matrix.coeff(2,b)); 24 | } 25 | 26 | template struct determinant_impl 29 | { 30 | static inline typename traits::Scalar run(const Derived& m) 31 | { 32 | if(Derived::ColsAtCompileTime==Dynamic && m.rows()==0) 33 | return typename traits::Scalar(1); 34 | return m.partialPivLu().determinant(); 35 | } 36 | }; 37 | 38 | template struct determinant_impl 39 | { 40 | static inline EIGEN_DEVICE_FUNC 41 | typename traits::Scalar run(const Derived& m) 42 | { 43 | return m.coeff(0,0); 44 | } 45 | }; 46 | 47 | template struct determinant_impl 48 | { 49 | static inline EIGEN_DEVICE_FUNC 50 | typename traits::Scalar run(const Derived& m) 51 | { 52 | return m.coeff(0,0) * m.coeff(1,1) - m.coeff(1,0) * m.coeff(0,1); 53 | } 54 | }; 55 | 56 | template struct determinant_impl 57 | { 58 | static inline EIGEN_DEVICE_FUNC 59 | typename traits::Scalar run(const Derived& m) 60 | { 61 | return bruteforce_det3_helper(m,0,1,2) 62 | - bruteforce_det3_helper(m,1,0,2) 63 | + bruteforce_det3_helper(m,2,0,1); 64 | } 65 | }; 66 | 67 | template struct determinant_impl 68 | { 69 | typedef typename traits::Scalar Scalar; 70 | static EIGEN_DEVICE_FUNC 71 | Scalar run(const Derived& m) 72 | { 73 | Scalar d2_01 = det2(m, 0, 1); 74 | Scalar d2_02 = det2(m, 0, 2); 75 | Scalar d2_03 = det2(m, 0, 3); 76 | Scalar d2_12 = det2(m, 1, 2); 77 | Scalar d2_13 = det2(m, 1, 3); 78 | Scalar d2_23 = det2(m, 2, 3); 79 | Scalar d3_0 = det3(m, 1,d2_23, 2,d2_13, 3,d2_12); 80 | Scalar d3_1 = det3(m, 0,d2_23, 2,d2_03, 3,d2_02); 81 | Scalar d3_2 = det3(m, 0,d2_13, 1,d2_03, 3,d2_01); 82 | Scalar d3_3 = det3(m, 0,d2_12, 1,d2_02, 2,d2_01); 83 | return internal::pmadd(-m(0,3),d3_0, m(1,3)*d3_1) + 84 | internal::pmadd(-m(2,3),d3_2, m(3,3)*d3_3); 85 | } 86 | protected: 87 | static EIGEN_DEVICE_FUNC 88 | Scalar det2(const Derived& m, Index i0, Index i1) 89 | { 90 | return m(i0,0) * m(i1,1) - m(i1,0) * m(i0,1); 91 | } 92 | 93 | static EIGEN_DEVICE_FUNC 94 | Scalar det3(const Derived& m, Index i0, const Scalar& d0, Index i1, const Scalar& d1, Index i2, const Scalar& d2) 95 | { 96 | return internal::pmadd(m(i0,2), d0, internal::pmadd(-m(i1,2), d1, m(i2,2)*d2)); 97 | } 98 | }; 99 | 100 | } // end namespace internal 101 | 102 | /** \lu_module 103 | * 104 | * \returns the determinant of this matrix 105 | */ 106 | template 107 | EIGEN_DEVICE_FUNC 108 | inline typename internal::traits::Scalar MatrixBase::determinant() const 109 | { 110 | eigen_assert(rows() == cols()); 111 | typedef typename internal::nested_eval::type Nested; 112 | return internal::determinant_impl::type>::run(derived()); 113 | } 114 | 115 | } // end namespace Eigen 116 | 117 | #endif // EIGEN_DETERMINANT_H 118 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/LU/PartialPivLU_LAPACKE.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Intel Corporation. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Intel Corporation nor the names of its contributors may 13 | be used to endorse or promote products derived from this software without 14 | specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 23 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | ******************************************************************************** 28 | * Content : Eigen bindings to LAPACKe 29 | * LU decomposition with partial pivoting based on LAPACKE_?getrf function. 30 | ******************************************************************************** 31 | */ 32 | 33 | #ifndef EIGEN_PARTIALLU_LAPACK_H 34 | #define EIGEN_PARTIALLU_LAPACK_H 35 | 36 | namespace Eigen { 37 | 38 | namespace internal { 39 | 40 | /** \internal Specialization for the data types supported by LAPACKe */ 41 | 42 | #define EIGEN_LAPACKE_LU_PARTPIV(EIGTYPE, LAPACKE_TYPE, LAPACKE_PREFIX) \ 43 | template \ 44 | struct partial_lu_impl \ 45 | { \ 46 | /* \internal performs the LU decomposition in-place of the matrix represented */ \ 47 | static lapack_int blocked_lu(Index rows, Index cols, EIGTYPE* lu_data, Index luStride, lapack_int* row_transpositions, lapack_int& nb_transpositions, lapack_int maxBlockSize=256) \ 48 | { \ 49 | EIGEN_UNUSED_VARIABLE(maxBlockSize);\ 50 | lapack_int matrix_order, first_zero_pivot; \ 51 | lapack_int m, n, lda, *ipiv, info; \ 52 | EIGTYPE* a; \ 53 | /* Set up parameters for ?getrf */ \ 54 | matrix_order = StorageOrder==RowMajor ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ 55 | lda = convert_index(luStride); \ 56 | a = lu_data; \ 57 | ipiv = row_transpositions; \ 58 | m = convert_index(rows); \ 59 | n = convert_index(cols); \ 60 | nb_transpositions = 0; \ 61 | \ 62 | info = LAPACKE_##LAPACKE_PREFIX##getrf( matrix_order, m, n, (LAPACKE_TYPE*)a, lda, ipiv ); \ 63 | \ 64 | for(int i=0;i= 0); \ 67 | /* something should be done with nb_transpositions */ \ 68 | \ 69 | first_zero_pivot = info; \ 70 | return first_zero_pivot; \ 71 | } \ 72 | }; 73 | 74 | EIGEN_LAPACKE_LU_PARTPIV(double, double, d) 75 | EIGEN_LAPACKE_LU_PARTPIV(float, float, s) 76 | EIGEN_LAPACKE_LU_PARTPIV(dcomplex, lapack_complex_double, z) 77 | EIGEN_LAPACKE_LU_PARTPIV(scomplex, lapack_complex_float, c) 78 | 79 | } // end namespace internal 80 | 81 | } // end namespace Eigen 82 | 83 | #endif // EIGEN_PARTIALLU_LAPACK_H 84 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/QR/HouseholderQR_LAPACKE.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Intel Corporation. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Intel Corporation nor the names of its contributors may 13 | be used to endorse or promote products derived from this software without 14 | specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 23 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | ******************************************************************************** 28 | * Content : Eigen bindings to LAPACKe 29 | * Householder QR decomposition of a matrix w/o pivoting based on 30 | * LAPACKE_?geqrf function. 31 | ******************************************************************************** 32 | */ 33 | 34 | #ifndef EIGEN_QR_LAPACKE_H 35 | #define EIGEN_QR_LAPACKE_H 36 | 37 | namespace Eigen { 38 | 39 | namespace internal { 40 | 41 | /** \internal Specialization for the data types supported by LAPACKe */ 42 | 43 | #define EIGEN_LAPACKE_QR_NOPIV(EIGTYPE, LAPACKE_TYPE, LAPACKE_PREFIX) \ 44 | template \ 45 | struct householder_qr_inplace_blocked \ 46 | { \ 47 | static void run(MatrixQR& mat, HCoeffs& hCoeffs, Index = 32, \ 48 | typename MatrixQR::Scalar* = 0) \ 49 | { \ 50 | lapack_int m = (lapack_int) mat.rows(); \ 51 | lapack_int n = (lapack_int) mat.cols(); \ 52 | lapack_int lda = (lapack_int) mat.outerStride(); \ 53 | lapack_int matrix_order = (MatrixQR::IsRowMajor) ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ 54 | LAPACKE_##LAPACKE_PREFIX##geqrf( matrix_order, m, n, (LAPACKE_TYPE*)mat.data(), lda, (LAPACKE_TYPE*)hCoeffs.data()); \ 55 | hCoeffs.adjointInPlace(); \ 56 | } \ 57 | }; 58 | 59 | EIGEN_LAPACKE_QR_NOPIV(double, double, d) 60 | EIGEN_LAPACKE_QR_NOPIV(float, float, s) 61 | EIGEN_LAPACKE_QR_NOPIV(dcomplex, lapack_complex_double, z) 62 | EIGEN_LAPACKE_QR_NOPIV(scomplex, lapack_complex_float, c) 63 | 64 | } // end namespace internal 65 | 66 | } // end namespace Eigen 67 | 68 | #endif // EIGEN_QR_LAPACKE_H 69 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/SparseCore/SparseDot.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSE_DOT_H 11 | #define EIGEN_SPARSE_DOT_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | template 17 | typename internal::traits::Scalar 18 | SparseMatrixBase::dot(const MatrixBase& other) const 19 | { 20 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 21 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) 22 | EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived) 23 | EIGEN_STATIC_ASSERT((internal::is_same::value), 24 | YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 25 | 26 | eigen_assert(size() == other.size()); 27 | eigen_assert(other.size()>0 && "you are using a non initialized vector"); 28 | 29 | internal::evaluator thisEval(derived()); 30 | typename internal::evaluator::InnerIterator i(thisEval, 0); 31 | Scalar res(0); 32 | while (i) 33 | { 34 | res += numext::conj(i.value()) * other.coeff(i.index()); 35 | ++i; 36 | } 37 | return res; 38 | } 39 | 40 | template 41 | template 42 | typename internal::traits::Scalar 43 | SparseMatrixBase::dot(const SparseMatrixBase& other) const 44 | { 45 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 46 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) 47 | EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived) 48 | EIGEN_STATIC_ASSERT((internal::is_same::value), 49 | YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 50 | 51 | eigen_assert(size() == other.size()); 52 | 53 | internal::evaluator thisEval(derived()); 54 | typename internal::evaluator::InnerIterator i(thisEval, 0); 55 | 56 | internal::evaluator otherEval(other.derived()); 57 | typename internal::evaluator::InnerIterator j(otherEval, 0); 58 | 59 | Scalar res(0); 60 | while (i && j) 61 | { 62 | if (i.index()==j.index()) 63 | { 64 | res += numext::conj(i.value()) * j.value(); 65 | ++i; ++j; 66 | } 67 | else if (i.index() 76 | inline typename NumTraits::Scalar>::Real 77 | SparseMatrixBase::squaredNorm() const 78 | { 79 | return numext::real((*this).cwiseAbs2().sum()); 80 | } 81 | 82 | template 83 | inline typename NumTraits::Scalar>::Real 84 | SparseMatrixBase::norm() const 85 | { 86 | using std::sqrt; 87 | return sqrt(squaredNorm()); 88 | } 89 | 90 | template 91 | inline typename NumTraits::Scalar>::Real 92 | SparseMatrixBase::blueNorm() const 93 | { 94 | return internal::blueNorm_impl(*this); 95 | } 96 | } // end namespace Eigen 97 | 98 | #endif // EIGEN_SPARSE_DOT_H 99 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/SparseCore/SparseTranspose.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2015 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSETRANSPOSE_H 11 | #define EIGEN_SPARSETRANSPOSE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | template 17 | class SparseTransposeImpl 18 | : public SparseMatrixBase > 19 | {}; 20 | 21 | template 22 | class SparseTransposeImpl 23 | : public SparseCompressedBase > 24 | { 25 | typedef SparseCompressedBase > Base; 26 | public: 27 | using Base::derived; 28 | typedef typename Base::Scalar Scalar; 29 | typedef typename Base::StorageIndex StorageIndex; 30 | 31 | inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); } 32 | 33 | inline const Scalar* valuePtr() const { return derived().nestedExpression().valuePtr(); } 34 | inline const StorageIndex* innerIndexPtr() const { return derived().nestedExpression().innerIndexPtr(); } 35 | inline const StorageIndex* outerIndexPtr() const { return derived().nestedExpression().outerIndexPtr(); } 36 | inline const StorageIndex* innerNonZeroPtr() const { return derived().nestedExpression().innerNonZeroPtr(); } 37 | 38 | inline Scalar* valuePtr() { return derived().nestedExpression().valuePtr(); } 39 | inline StorageIndex* innerIndexPtr() { return derived().nestedExpression().innerIndexPtr(); } 40 | inline StorageIndex* outerIndexPtr() { return derived().nestedExpression().outerIndexPtr(); } 41 | inline StorageIndex* innerNonZeroPtr() { return derived().nestedExpression().innerNonZeroPtr(); } 42 | }; 43 | } 44 | 45 | template class TransposeImpl 46 | : public internal::SparseTransposeImpl 47 | { 48 | protected: 49 | typedef internal::SparseTransposeImpl Base; 50 | }; 51 | 52 | namespace internal { 53 | 54 | template 55 | struct unary_evaluator, IteratorBased> 56 | : public evaluator_base > 57 | { 58 | typedef typename evaluator::InnerIterator EvalIterator; 59 | public: 60 | typedef Transpose XprType; 61 | 62 | inline Index nonZerosEstimate() const { 63 | return m_argImpl.nonZerosEstimate(); 64 | } 65 | 66 | class InnerIterator : public EvalIterator 67 | { 68 | public: 69 | EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer) 70 | : EvalIterator(unaryOp.m_argImpl,outer) 71 | {} 72 | 73 | Index row() const { return EvalIterator::col(); } 74 | Index col() const { return EvalIterator::row(); } 75 | }; 76 | 77 | enum { 78 | CoeffReadCost = evaluator::CoeffReadCost, 79 | Flags = XprType::Flags 80 | }; 81 | 82 | explicit unary_evaluator(const XprType& op) :m_argImpl(op.nestedExpression()) {} 83 | 84 | protected: 85 | evaluator m_argImpl; 86 | }; 87 | 88 | } // end namespace internal 89 | 90 | } // end namespace Eigen 91 | 92 | #endif // EIGEN_SPARSETRANSPOSE_H 93 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/SparseLU/SparseLU_copy_to_ucol.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 | * NOTE: This file is the modified version of [s,d,c,z]copy_to_ucol.c file in SuperLU 12 | 13 | * -- SuperLU routine (version 2.0) -- 14 | * Univ. of California Berkeley, Xerox Palo Alto Research Center, 15 | * and Lawrence Berkeley National Lab. 16 | * November 15, 1997 17 | * 18 | * Copyright (c) 1994 by Xerox Corporation. All rights reserved. 19 | * 20 | * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY 21 | * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. 22 | * 23 | * Permission is hereby granted to use or copy this program for any 24 | * purpose, provided the above notices are retained on all copies. 25 | * Permission to modify the code and to distribute modified code is 26 | * granted, provided the above notices are retained, and a notice that 27 | * the code was modified is included with the above copyright notice. 28 | */ 29 | #ifndef SPARSELU_COPY_TO_UCOL_H 30 | #define SPARSELU_COPY_TO_UCOL_H 31 | 32 | namespace Eigen { 33 | namespace internal { 34 | 35 | /** 36 | * \brief Performs numeric block updates (sup-col) in topological order 37 | * 38 | * \param jcol current column to update 39 | * \param nseg Number of segments in the U part 40 | * \param segrep segment representative ... 41 | * \param repfnz First nonzero column in each row ... 42 | * \param perm_r Row permutation 43 | * \param dense Store the full representation of the column 44 | * \param glu Global LU data. 45 | * \return 0 - successful return 46 | * > 0 - number of bytes allocated when run out of space 47 | * 48 | */ 49 | template 50 | Index SparseLUImpl::copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep, 51 | BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu) 52 | { 53 | Index ksub, krep, ksupno; 54 | 55 | Index jsupno = glu.supno(jcol); 56 | 57 | // For each nonzero supernode segment of U[*,j] in topological order 58 | Index k = nseg - 1, i; 59 | StorageIndex nextu = glu.xusub(jcol); 60 | Index kfnz, isub, segsize; 61 | Index new_next,irow; 62 | Index fsupc, mem; 63 | for (ksub = 0; ksub < nseg; ksub++) 64 | { 65 | krep = segrep(k); k--; 66 | ksupno = glu.supno(krep); 67 | if (jsupno != ksupno ) // should go into ucol(); 68 | { 69 | kfnz = repfnz(krep); 70 | if (kfnz != emptyIdxLU) 71 | { // Nonzero U-segment 72 | fsupc = glu.xsup(ksupno); 73 | isub = glu.xlsub(fsupc) + kfnz - fsupc; 74 | segsize = krep - kfnz + 1; 75 | new_next = nextu + segsize; 76 | while (new_next > glu.nzumax) 77 | { 78 | mem = memXpand(glu.ucol, glu.nzumax, nextu, UCOL, glu.num_expansions); 79 | if (mem) return mem; 80 | mem = memXpand(glu.usub, glu.nzumax, nextu, USUB, glu.num_expansions); 81 | if (mem) return mem; 82 | 83 | } 84 | 85 | for (i = 0; i < segsize; i++) 86 | { 87 | irow = glu.lsub(isub); 88 | glu.usub(nextu) = perm_r(irow); // Unlike the L part, the U part is stored in its final order 89 | glu.ucol(nextu) = dense(irow); 90 | dense(irow) = Scalar(0.0); 91 | nextu++; 92 | isub++; 93 | } 94 | 95 | } // end nonzero U-segment 96 | 97 | } // end if jsupno 98 | 99 | } // end for each segment 100 | glu.xusub(jcol + 1) = nextu; // close U(*,jcol) 101 | return 0; 102 | } 103 | 104 | } // namespace internal 105 | } // end namespace Eigen 106 | 107 | #endif // SPARSELU_COPY_TO_UCOL_H 108 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/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 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Eigen/src/plugins/MatrixCwiseUnaryOps.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | // This file is included into the body of the base classes supporting matrix specific coefficient-wise functions. 12 | // This include MatrixBase and SparseMatrixBase. 13 | 14 | 15 | typedef CwiseUnaryOp, const Derived> CwiseAbsReturnType; 16 | typedef CwiseUnaryOp, const Derived> CwiseAbs2ReturnType; 17 | typedef CwiseUnaryOp, const Derived> CwiseArgReturnType; 18 | typedef CwiseUnaryOp, const Derived> CwiseSqrtReturnType; 19 | typedef CwiseUnaryOp, const Derived> CwiseSignReturnType; 20 | typedef CwiseUnaryOp, const Derived> CwiseInverseReturnType; 21 | 22 | /// \returns an expression of the coefficient-wise absolute value of \c *this 23 | /// 24 | /// Example: \include MatrixBase_cwiseAbs.cpp 25 | /// Output: \verbinclude MatrixBase_cwiseAbs.out 26 | /// 27 | EIGEN_DOC_UNARY_ADDONS(cwiseAbs,absolute value) 28 | /// 29 | /// \sa cwiseAbs2() 30 | /// 31 | EIGEN_DEVICE_FUNC 32 | EIGEN_STRONG_INLINE const CwiseAbsReturnType 33 | cwiseAbs() const { return CwiseAbsReturnType(derived()); } 34 | 35 | /// \returns an expression of the coefficient-wise squared absolute value of \c *this 36 | /// 37 | /// Example: \include MatrixBase_cwiseAbs2.cpp 38 | /// Output: \verbinclude MatrixBase_cwiseAbs2.out 39 | /// 40 | EIGEN_DOC_UNARY_ADDONS(cwiseAbs2,squared absolute value) 41 | /// 42 | /// \sa cwiseAbs() 43 | /// 44 | EIGEN_DEVICE_FUNC 45 | EIGEN_STRONG_INLINE const CwiseAbs2ReturnType 46 | cwiseAbs2() const { return CwiseAbs2ReturnType(derived()); } 47 | 48 | /// \returns an expression of the coefficient-wise square root of *this. 49 | /// 50 | /// Example: \include MatrixBase_cwiseSqrt.cpp 51 | /// Output: \verbinclude MatrixBase_cwiseSqrt.out 52 | /// 53 | EIGEN_DOC_UNARY_ADDONS(cwiseSqrt,square-root) 54 | /// 55 | /// \sa cwisePow(), cwiseSquare() 56 | /// 57 | EIGEN_DEVICE_FUNC 58 | inline const CwiseSqrtReturnType 59 | cwiseSqrt() const { return CwiseSqrtReturnType(derived()); } 60 | 61 | /// \returns an expression of the coefficient-wise signum of *this. 62 | /// 63 | /// Example: \include MatrixBase_cwiseSign.cpp 64 | /// Output: \verbinclude MatrixBase_cwiseSign.out 65 | /// 66 | EIGEN_DOC_UNARY_ADDONS(cwiseSign,sign function) 67 | /// 68 | EIGEN_DEVICE_FUNC 69 | inline const CwiseSignReturnType 70 | cwiseSign() const { return CwiseSignReturnType(derived()); } 71 | 72 | 73 | /// \returns an expression of the coefficient-wise inverse of *this. 74 | /// 75 | /// Example: \include MatrixBase_cwiseInverse.cpp 76 | /// Output: \verbinclude MatrixBase_cwiseInverse.out 77 | /// 78 | EIGEN_DOC_UNARY_ADDONS(cwiseInverse,inverse) 79 | /// 80 | /// \sa cwiseProduct() 81 | /// 82 | EIGEN_DEVICE_FUNC 83 | inline const CwiseInverseReturnType 84 | cwiseInverse() const { return CwiseInverseReturnType(derived()); } 85 | 86 | /// \returns an expression of the coefficient-wise phase angle of \c *this 87 | /// 88 | /// Example: \include MatrixBase_cwiseArg.cpp 89 | /// Output: \verbinclude MatrixBase_cwiseArg.out 90 | /// 91 | EIGEN_DOC_UNARY_ADDONS(cwiseArg,arg) 92 | 93 | EIGEN_DEVICE_FUNC 94 | inline const CwiseArgReturnType 95 | cwiseArg() const { return CwiseArgReturnType(derived()); } 96 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Estimator2001_Kalman.cpp: -------------------------------------------------------------------------------- 1 | #include "Estimator2001_Kalman.h" 2 | #include 3 | #include "Eigen/Dense" 4 | 5 | using namespace Eigen; 6 | 7 | static MatrixXd E2001_F; 8 | static MatrixXd E2001_H; 9 | static MatrixXd E2001_Q; 10 | static MatrixXd E2001_R; 11 | static MatrixXd E2001_P; 12 | static MatrixXd E2001_K; 13 | 14 | static VectorXd E2001_Xe; 15 | static VectorXd E2001_Xp; 16 | static VectorXd E2001_Ze; 17 | static VectorXd E2001_Z; 18 | 19 | void Estimator2001_Init(EstimatorPortN *estimator) 20 | { 21 | std::cout << "Linear Kalman estimator is initialized" << std::endl; 22 | } 23 | 24 | void Estimator2001_Estimation(EstimatorPortN *estimator) 25 | { 26 | // Map input data to Eigen objects 27 | E2001_Z = Map>(estimator->CurrentObservation, estimator->Nz); 28 | E2001_Xe = Map>(estimator->EstimatedState, estimator->Nx); 29 | E2001_P = Map>(estimator->Matrix_P, estimator->Nx, estimator->Nx); 30 | E2001_F = Map>(estimator->Matrix_F, estimator->Nx, estimator->Nx); 31 | E2001_Q = Map>(estimator->Matrix_Q, estimator->Nx, estimator->Nx); 32 | E2001_H = Map>(estimator->Matrix_H, estimator->Nz, estimator->Nx); 33 | E2001_R = Map>(estimator->Matrix_R, estimator->Nz, estimator->Nz); 34 | 35 | 36 | // State prediction 37 | E2001_Xe = E2001_F * E2001_Xe; 38 | 39 | // Covariance prediction 40 | E2001_P = E2001_F * E2001_P * E2001_F.transpose() + E2001_Q; 41 | 42 | // Calculate Kalman gain 43 | MatrixXd S = E2001_H * E2001_P * E2001_H.transpose() + E2001_R; 44 | E2001_K = E2001_P * E2001_H.transpose() * S.inverse(); 45 | 46 | // State update 47 | VectorXd y = E2001_Z - E2001_H * E2001_Xe; 48 | E2001_Xe = E2001_Xe + E2001_K * y; 49 | 50 | // Update covariance matrix 51 | E2001_P = (MatrixXd::Identity(estimator->Nx, estimator->Nx) - E2001_K * E2001_H) * E2001_P; 52 | 53 | // State prediction (multi-step prediction) 54 | E2001_Xp = E2001_Xe; 55 | for (int i = 0; i < estimator->PredictStep; ++i) 56 | { 57 | E2001_Xp = E2001_F * E2001_Xp; 58 | } 59 | 60 | // Observation prediction 61 | E2001_Ze = E2001_H * E2001_Xp; 62 | 63 | // Output results 64 | Map(estimator->EstimatedState, estimator->Nx) = E2001_Xe; 65 | Map(estimator->PredictedState, estimator->Nx) = E2001_Xp; 66 | Map(estimator->PredictedObservation, estimator->Nz) = E2001_Ze; 67 | Map>(estimator->Matrix_P, estimator->Nx, estimator->Nx) = E2001_P; 68 | } 69 | 70 | void Estimator2001_Termination() 71 | { 72 | std::cout << "Estimator2001 terminated." << std::endl; 73 | } 74 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Estimator2001_Kalman.h: -------------------------------------------------------------------------------- 1 | /* 2 | % =========================================================================== 3 | % Kalman Estimator ID 2001 4 | % Filename: Estimator2001_Kalman.cpp Estimator2001_Kalman.h 5 | % Description: 6 | % This function estimator performs linear estimation based on the structure provided in EstimatorPortN.h and the specific state-space model provided in StateSpaceModelN.c 7 | % 8 | % Initial version: Minxing Sun 9 | % Unit: UCAS, Institute of Optics And Electronics, Lab 1, Class of 2020 10 | % Email: 401435318@qq.com 11 | % Date: November 16, 2024 12 | % 13 | % Updates: 14 | % Unit: 15 | % Email: 16 | % Date: 17 | % =========================================================================== 18 | */ 19 | #ifndef _ESTIMATOR2001_KALMAN_H_ 20 | #define _ESTIMATOR2001_KALMAN_H_ 21 | 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | #include "../EstimatorPortN.h" 28 | 29 | void Estimator2001_Init(EstimatorPortN *estimator); 30 | void Estimator2001_Estimation(EstimatorPortN *estimator); 31 | void Estimator2001_Termination(); 32 | 33 | #ifdef __cplusplus 34 | } 35 | #endif 36 | 37 | #endif -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Cpp_Estimators/Estimator2002_UnscentedKalmanEstiamtor.h: -------------------------------------------------------------------------------- 1 | /* 2 | % =========================================================================== 3 | % Unscented Kalman Estimator ID 2002 4 | % Filename: Estimator2002_UnscentedKalmanEstimator.cpp Estimator2002_UnscentedKalmanEstimator.h 5 | % Description: 6 | % This estimator performs nonlinear estimation based on the structure provided in EstimatorPortN.h and the specific state-space model provided in StateSpaceModelN.c 7 | % 8 | % Initial version: Minxing Sun 9 | % Unit: UCAS, Institute of Optics And Electronics, Lab 1, Class of 2020 10 | % Email: 401435318@qq.com 11 | % Date: November 16, 2024 12 | % 13 | % Updates: 14 | % Unit: 15 | % Email: 16 | % Date: 17 | % =========================================================================== 18 | */ 19 | #ifndef _Estimator2002_H_ 20 | #define _Estimator2002_H_ 21 | 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | #include "../EstimatorPortN.h" 28 | 29 | void Estimator2002_Init(EstimatorPortN *estimator); 30 | void Estimator2002_Estimation(EstimatorPortN *estimator); 31 | void Estimator2002_Termination(); 32 | 33 | #ifdef __cplusplus 34 | } 35 | #endif 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Estimator/Readme.md: -------------------------------------------------------------------------------- 1 | This is a pure C/C++ Estimator Port, could be used in any platform. -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Readme.md: -------------------------------------------------------------------------------- 1 | This repository is a pure C/C++ program, doesn't contain fusion_estimator_node.cpp or other ROS file. Which means you can use it in both ROS1 ROS2 and other platform. -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/SensorBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | Author：Sun Minxing 3 | School: Institute of Optics And Electronics, Chinese Academy of Science 4 | Email： 401435318@qq.com 5 | */ 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "Estimator/EstimatorPortN.h" 14 | #include "Estimator/Cpp_Estimators/Eigen/Dense" 15 | 16 | using namespace Eigen; 17 | 18 | namespace DataFusion 19 | { 20 | extern Eigen::Quaterniond Est_Quaternion; // Body Orientation Quaternion 21 | extern Eigen::Quaterniond Est_QuaternionInv; // Body Orientation Quaternion Inverse 22 | 23 | class Sensors 24 | { 25 | public: 26 | 27 | Sensors(EstimatorPortN* StateSpaceModel_) 28 | { 29 | std::cout << std::fixed << std::setprecision(4); 30 | 31 | StateSpaceModel = StateSpaceModel_; 32 | SensorQuaternion = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); 33 | SensorQuaternionInv = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); 34 | 35 | for(int i = 0; i < StateSpaceModel->Nz; i++) 36 | StateSpaceModel->Matrix_R[i*StateSpaceModel->Nz+i] = R_diag[i]; 37 | 38 | Est_Quaternion.setIdentity(); 39 | Est_QuaternionInv.setIdentity(); 40 | } 41 | 42 | virtual ~Sensors() = default; 43 | virtual void SensorDataHandle(double* Message, double Time) {} 44 | double SensorPosition[3] = {0,0,0}; 45 | Eigen::Quaterniond SensorQuaternion; 46 | Eigen::Quaterniond SensorQuaternionInv; 47 | 48 | protected: 49 | 50 | EstimatorPortN* StateSpaceModel; 51 | Eigen::Quaterniond Est_QuaternionTemp1, Est_QuaternionTemp2, Est_QuaternionTemp3; 52 | Eigen::Vector3d Est_BodyAngleVel, Est_SensorWorldPosition, Est_SensorWorldVelocity, Est_SensorPosition, Est_Vector3dTemp1, Est_Vector3dTemp2; 53 | 54 | double Observation[9] = {0}; 55 | double ObservationTime = 0; 56 | double R_diag[9] = {1,1,1,1,1,1,1,1,1}; 57 | 58 | void UpdateEst_Quaternion(); 59 | void ObservationCorrect_Position(); 60 | void ObservationCorrect_Velocity(); 61 | void ObservationCorrect_Acceleration(); 62 | void ObservationCorrect_Orientation(); 63 | void ObservationCorrect_AngularVelocity(); 64 | void ObservationCorrect_AngularAcceleration(); 65 | 66 | }; 67 | } -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Sensor_IMU.cpp: -------------------------------------------------------------------------------- 1 | #include "Sensor_IMU.h" 2 | 3 | namespace DataFusion 4 | { 5 | void SensorIMUAcc::SensorDataHandle(double* Message, double Time) 6 | { 7 | 8 | ObservationTime = Time; 9 | // std::cout << "Flag1 " << std::endl; 10 | for(int i = 0; i < 9; i++ ) 11 | Observation[i] = Message[i]; 12 | 13 | for(int i = 0; i < StateSpaceModel->Nx * StateSpaceModel->Nz; i++) 14 | { 15 | StateSpaceModel->Matrix_H[i] = 0; 16 | } 17 | for(int i = 0; i < 3; i++) 18 | { 19 | StateSpaceModel->Matrix_H[(3 * i + 2) * StateSpaceModel->Nx + (3 * i + 2)] = 1; 20 | } 21 | 22 | ObservationCorrect_Acceleration(); 23 | Observation[8] -= 9.6; 24 | 25 | StateSpaceModel1_EstimatorPort(Observation, ObservationTime, StateSpaceModel); 26 | 27 | } 28 | 29 | void SensorIMUMagGyro::SensorDataHandle(double* Message, double Time){ 30 | 31 | ObservationTime = Time; 32 | for(int i = 0; i < 9; i++ ) 33 | Observation[i] = Message[i]; 34 | 35 | for(int i = 0; i < StateSpaceModel->Nx * StateSpaceModel->Nz; i++) 36 | { 37 | StateSpaceModel->Matrix_H[i] = 0; 38 | } 39 | for(int i = 0; i < 3; i++) 40 | { 41 | StateSpaceModel->Matrix_H[(3 * i + 0) * StateSpaceModel->Nx + (3 * i + 0)] = 1; 42 | StateSpaceModel->Matrix_H[(3 * i + 1) * StateSpaceModel->Nx + (3 * i + 1)] = 1; 43 | } 44 | 45 | ObservationCorrect_Orientation(); 46 | ObservationCorrect_AngularVelocity(); 47 | OrientationCorrect(); 48 | 49 | 50 | StateSpaceModel2_EstimatorPort(Observation, ObservationTime, StateSpaceModel); 51 | 52 | UpdateEst_Quaternion(); 53 | } 54 | 55 | void SensorIMUMagGyro::OrientationCorrect(){ 56 | 57 | static int Est_OriRollTurningTimesRecord = 0, Est_OriYawTurningTimesRecord = 0, Est_OriPitchTurningTimesRecord = 0; 58 | static double Est_OrientationRecord[3] = {0}; 59 | 60 | if (sin(Est_OrientationRecord[0]) >= 0 && Observation[0]< -0.9 * M_PI) 61 | Est_OriRollTurningTimesRecord += 1; 62 | if (sin(Est_OrientationRecord[0]) < 0 && Observation[0]> 0.9 * M_PI) 63 | Est_OriRollTurningTimesRecord -= 1; 64 | 65 | if (sin(Est_OrientationRecord[1]) >= 0 && Observation[3]< -0.9 * M_PI) 66 | Est_OriPitchTurningTimesRecord += 1; 67 | if (sin(Est_OrientationRecord[1]) < 0 && Observation[3]> 0.9 * M_PI) 68 | Est_OriPitchTurningTimesRecord -= 1; 69 | 70 | if (sin(Est_OrientationRecord[2]) >= 0 && Observation[6]< -0.9 * M_PI) 71 | Est_OriYawTurningTimesRecord += 1; 72 | if (sin(Est_OrientationRecord[2]) < 0 && Observation[6] > 0.9 * M_PI) 73 | Est_OriYawTurningTimesRecord -= 1; 74 | 75 | Est_OrientationRecord[0] = Observation[0]; 76 | Est_OrientationRecord[1] = Observation[3]; 77 | Est_OrientationRecord[2] = Observation[6]; 78 | 79 | Observation[0] = (Observation[0]+ Est_OriRollTurningTimesRecord * 2 * M_PI); 80 | Observation[3] = (Observation[3]+ Est_OriPitchTurningTimesRecord * 2 * M_PI); 81 | Observation[6] = (Observation[6]+ Est_OriYawTurningTimesRecord * 2 * M_PI); 82 | 83 | } 84 | } -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Sensor_IMU.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "SensorBase.h" 4 | 5 | using namespace Eigen; 6 | 7 | namespace DataFusion 8 | { 9 | class SensorIMUAcc : public Sensors 10 | { 11 | public: 12 | 13 | SensorIMUAcc(EstimatorPortN* StateSpaceModel_):Sensors(StateSpaceModel_) 14 | { 15 | double AngleCorrect[3] = {0}; 16 | AngleCorrect[0] = 0 * M_PI / 180.0; 17 | AngleCorrect[1] = 1.7 * M_PI / 180.0; 18 | AngleCorrect[2] = 0 * M_PI / 180.0; 19 | Eigen::AngleAxisd rollAngle(AngleCorrect[2], Eigen::Vector3d::UnitZ()); // 绕 X 轴旋转 20 | Eigen::AngleAxisd pitchAngle(AngleCorrect[1], Eigen::Vector3d::UnitY()); // 绕 Y 轴旋转 21 | Eigen::AngleAxisd yawAngle(AngleCorrect[0], Eigen::Vector3d::UnitX()); // 绕 Z 轴旋转 22 | SensorQuaternion = yawAngle * pitchAngle * rollAngle; 23 | SensorQuaternionInv = SensorQuaternion.inverse(); 24 | } 25 | 26 | void SensorDataHandle(double* Message, double Time) override; 27 | 28 | protected: 29 | 30 | }; 31 | 32 | class SensorIMUMagGyro : public Sensors 33 | { 34 | public: 35 | 36 | SensorIMUMagGyro(EstimatorPortN* StateSpaceModel_):Sensors(StateSpaceModel_) 37 | { 38 | double AngleCorrect[3] = {0}; 39 | AngleCorrect[0] = 0 * M_PI / 180.0; 40 | AngleCorrect[1] = 1.7 * M_PI / 180.0; 41 | AngleCorrect[2] = 0 * M_PI / 180.0; 42 | Eigen::AngleAxisd rollAngle(AngleCorrect[0], Eigen::Vector3d::UnitX()); // 绕 X 轴旋转 43 | Eigen::AngleAxisd pitchAngle(AngleCorrect[1], Eigen::Vector3d::UnitY()); // 绕 Y 轴旋转 44 | Eigen::AngleAxisd yawAngle(AngleCorrect[2], Eigen::Vector3d::UnitZ()); // 绕 Z 轴旋转 45 | SensorQuaternion = yawAngle * pitchAngle * rollAngle; 46 | SensorQuaternionInv = SensorQuaternion.inverse(); 47 | } 48 | 49 | void SensorDataHandle(double* Message, double Time) override; 50 | void OrientationCorrect(); 51 | 52 | protected: 53 | 54 | }; 55 | } -------------------------------------------------------------------------------- /fusion_estimator/src/GO2FusionEstimator/Sensor_Legs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "SensorBase.h" 4 | 5 | using namespace Eigen; 6 | 7 | namespace DataFusion 8 | { 9 | class SensorLegs : public Sensors 10 | { 11 | public: 12 | 13 | SensorLegs(EstimatorPortN* StateSpaceModel_):Sensors(StateSpaceModel_) 14 | { 15 | for(int i=0;i<4;i++) 16 | { 17 | FootIsOnGround[i] = 1; 18 | FootWasOnGround[i] = 1; 19 | FootLanding[i] = 0; 20 | } 21 | } 22 | 23 | void SensorDataHandle(double* Message, double Time) override; 24 | 25 | Eigen::Matrix KinematicParams; 26 | double Par_HipLength, Par_ThighLength, Par_CalfLength, Par_FootLength; 27 | int FootfallPositionRecordIsInitiated[4] = {0}; 28 | 29 | protected: 30 | 31 | double FootEffortThreshold = 30; 32 | bool FootIsOnGround[4], FootWasOnGround[4], FootLanding[4]; 33 | int LatestFeetEffort; 34 | double FootfallPositionRecord[4][3]={0}; 35 | 36 | void Joint2HipFoot(int LegNumber); 37 | void PositionCorrect(int LegnNmber); 38 | 39 | }; 40 | } -------------------------------------------------------------------------------- /message_handle/message_handle/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShineMinxing/Ros2Go2Estimator/e2d1dcb0f07ba28927c380c6938216728a6efb10/message_handle/message_handle/__init__.py -------------------------------------------------------------------------------- /message_handle/message_handle/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShineMinxing/Ros2Go2Estimator/e2d1dcb0f07ba28927c380c6938216728a6efb10/message_handle/message_handle/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /message_handle/message_handle/__pycache__/message_handle.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShineMinxing/Ros2Go2Estimator/e2d1dcb0f07ba28927c380c6938216728a6efb10/message_handle/message_handle/__pycache__/message_handle.cpython-310.pyc -------------------------------------------------------------------------------- /message_handle/message_handle/__pycache__/message_handle_node.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShineMinxing/Ros2Go2Estimator/e2d1dcb0f07ba28927c380c6938216728a6efb10/message_handle/message_handle/__pycache__/message_handle_node.cpython-310.pyc -------------------------------------------------------------------------------- /message_handle/message_handle/__pycache__/pointcloud_to_laserscan_custom.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShineMinxing/Ros2Go2Estimator/e2d1dcb0f07ba28927c380c6938216728a6efb10/message_handle/message_handle/__pycache__/pointcloud_to_laserscan_custom.cpython-310.pyc -------------------------------------------------------------------------------- /message_handle/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | message_handle 5 | 0.0.0 6 | TODO: Package description 7 | smx 8 | TODO: License declaration 9 | 10 | rclpy 11 | sensor_msgs 12 | tf2_ros 13 | tf2_sensor_msgs 14 | 15 | ament_copyright 16 | ament_flake8 17 | ament_pep257 18 | python3-pytest 19 | 20 | sensor_msgs_py 21 | 22 | 23 | ament_python 24 | 25 | 26 | -------------------------------------------------------------------------------- /message_handle/resource/message_handle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShineMinxing/Ros2Go2Estimator/e2d1dcb0f07ba28927c380c6938216728a6efb10/message_handle/resource/message_handle -------------------------------------------------------------------------------- /message_handle/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/message_handle 3 | [install] 4 | install_scripts=$base/lib/message_handle 5 | -------------------------------------------------------------------------------- /message_handle/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup( 4 | name='message_handle', 5 | version='0.0.1', # 明确版本号 6 | packages=find_packages(), # 自动发现包结构 7 | data_files=[ 8 | ('share/ament_index/resource_index/packages', 9 | ['resource/message_handle']), 10 | ('share/message_handle', ['package.xml']), 11 | ], 12 | install_requires=['setuptools', 'numpy'], 13 | zip_safe=True, 14 | maintainer='your_name', 15 | maintainer_email='your_email@example.com', 16 | description='PointCloud to LaserScan converter', 17 | license='Apache License 2.0', 18 | tests_require=['pytest'], 19 | entry_points={ 20 | 'console_scripts': [ 21 | 'message_handle_node = message_handle.message_handle_node:main', 22 | ], 23 | }, 24 | ) -------------------------------------------------------------------------------- /message_handle/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /message_handle/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /message_handle/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /other/slam_params.yaml: -------------------------------------------------------------------------------- 1 | slam_toolbox: 2 | ros__parameters: 3 | 4 | # 重要的帧和话题名称 5 | map_frame: map 6 | odom_frame: odom 7 | base_frame: base_link_2D 8 | scan_topic: /SMX/Scan 9 | odom_topic: /SMX/Odom_2D 10 | odom_linear_covariance: 0.1 # 里程计线速度噪声 11 | odom_angular_covariance: 0.1 # 里程计角速度噪声 12 | use_odom_motion: true # 是否使用里程计运动模型 13 | 14 | # TF发布频率 (Hz) 15 | transform_publish_period: 0.1 16 | transform_timeout: 0.1 17 | tf_message_filter_queue_size: 200 18 | 19 | # 激光扫描匹配相关参数 20 | max_laser_range: 30.0 # 激光雷达最大范围 (单位:米) 21 | min_laser_range: 0.20 # 激光雷达最小范围 22 | max_iterations: 500 # 匹配最大迭代次数 23 | minimum_travel_distance: 0.05 24 | minimum_travel_heading: 0.05 25 | 26 | # 绘图 27 | map_update_interval: 0.1 28 | resolution: 0.25 29 | 30 | # 优化参数 31 | use_scan_matching: false 32 | use_scan_barycenter: false 33 | do_loop_closing: true 34 | position_covariance_scale: 1.0 35 | yaw_covariance_scale: 1.0 36 | loop_match_minimum_response_fine: 0.5 37 | distance_variance_penalty: 0.01 38 | angle_variance_penalty: 0.01 39 | optimization_period: 0.5 # 优化间隔 (秒) 40 | loop_search_max_distance: 0.5 # 回环检测最大距离 (米) 41 | 42 | # CPU和内存控制 43 | throttle_scans: 1 44 | solver_plugin: solver_plugins::CeresSolver -------------------------------------------------------------------------------- /sport_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sport_control) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_action REQUIRED) 8 | find_package(nav2_msgs REQUIRED) 9 | find_package(geometry_msgs REQUIRED) 10 | find_package(sensor_msgs REQUIRED) 11 | find_package(unitree_sdk2 REQUIRED) 12 | find_package(Eigen3 REQUIRED) 13 | find_package(tf2 REQUIRED) 14 | find_package(tf2_ros REQUIRED) 15 | 16 | add_executable(sport_control_node src/sport_control_node.cpp) 17 | 18 | target_include_directories(sport_control_node 19 | PRIVATE 20 | ${rclcpp_INCLUDE_DIRS} 21 | ${sensor_msgs_INCLUDE_DIRS} 22 | ${unitree_sdk2_INCLUDE_DIRS} 23 | ${EIGEN3_INCLUDE_DIR} 24 | ) 25 | 26 | target_link_libraries(sport_control_node 27 | ${ament_LIBRARIES} 28 | unitree_sdk2 29 | ddsc ddscxx 30 | ) 31 | 32 | ament_target_dependencies(sport_control_node 33 | rclcpp 34 | sensor_msgs 35 | rclcpp_action 36 | nav2_msgs 37 | geometry_msgs 38 | tf2 39 | tf2_ros 40 | ) 41 | 42 | install(TARGETS 43 | sport_control_node 44 | DESTINATION lib/${PROJECT_NAME} 45 | ) 46 | 47 | install( 48 | DIRECTORY launch/ 49 | DESTINATION share/${PROJECT_NAME}/launch 50 | ) 51 | 52 | ament_package() -------------------------------------------------------------------------------- /sport_control/launch/camera_launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch_ros.actions import Node 3 | from launch import LaunchDescription 4 | from launch.actions import ExecuteProcess, TimerAction # 新增 TimerAction 5 | 6 | def generate_launch_description(): 7 | return LaunchDescription([ 8 | ExecuteProcess( 9 | cmd=[ 10 | "x-terminal-emulator", 11 | "--new-process", 12 | "-e", 13 | 'bash', 14 | '-c', 15 | 'source ~/.bashrc && ' 16 | 'ros2 run image_transport republish compressed raw --ros-args --remap in/compressed:=/SMX/GimbalCamera_Compressed --remap out:=/SMX/GimbalCamera & ' 17 | 'read -p "Press enter to close"' 18 | ], 19 | output="screen", 20 | ), 21 | ] 22 | ) -------------------------------------------------------------------------------- /sport_control/launch/g1_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | # camera 节点 7 | Node( 8 | package='g1_camera', 9 | executable='g1_camera_node', 10 | output='screen', 11 | ), 12 | # gimbal 节点 13 | Node( 14 | package='g1_gimbal', 15 | executable='g1_gimbal_node', 16 | output='screen', 17 | ), 18 | ]) 19 | -------------------------------------------------------------------------------- /sport_control/launch/go2_launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch_ros.actions import Node 3 | from launch import LaunchDescription 4 | from launch.actions import ExecuteProcess, TimerAction 5 | 6 | def generate_launch_description(): 7 | return LaunchDescription([ 8 | ExecuteProcess( 9 | cmd=[ 10 | "x-terminal-emulator", 11 | "--new-process", 12 | "-e", 13 | 'bash', 14 | '-c', 15 | 'source ~/.bashrc && ' 16 | 'ros2 run joy joy_node & ' 17 | 'ros2 run sport_control sport_control_node; ' 18 | 'read -p "Press enter to close"' 19 | ], 20 | output="screen", 21 | ), 22 | 23 | TimerAction( 24 | period=1.0, 25 | actions=[ 26 | ExecuteProcess( 27 | cmd=[ 28 | "x-terminal-emulator", 29 | "--new-process", 30 | "-e", 31 | 'bash', 32 | '-c', 33 | 'source ~/.bashrc && ' 34 | 'ros2 run fusion_estimator fusion_estimator_node;' 35 | 'read -p "Press enter to close"' 36 | ], 37 | output="screen", 38 | ), 39 | ] 40 | ), 41 | 42 | TimerAction( 43 | period=2.0, 44 | actions=[ 45 | ExecuteProcess( 46 | cmd=[ 47 | "x-terminal-emulator", 48 | "--new-process", 49 | "-e", 50 | 'bash', 51 | '-c', 52 | 'source ~/.bashrc && ' 53 | 'ros2 run dds_rostopic dds_rostopic_node & ' 54 | 'ros2 run message_handle message_handle_node; ' 55 | 'read -p "Press enter to close"' 56 | ], 57 | output="screen", 58 | ), 59 | ] 60 | ), 61 | ] 62 | ) -------------------------------------------------------------------------------- /sport_control/launch/nav_launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch_ros.actions import Node 3 | from launch import LaunchDescription 4 | from launch.actions import ExecuteProcess, TimerAction # 新增 TimerAction 5 | 6 | def generate_launch_description(): 7 | return LaunchDescription([ 8 | ExecuteProcess( 9 | cmd=[ 10 | "x-terminal-emulator", 11 | "--new-process", 12 | "-e", 13 | 'bash', 14 | '-c', 15 | 'source ~/.bashrc && ' 16 | 'ros2 launch nav2_bringup bringup_launch.py ' 17 | 'map:=/home/unitree/ros2_ws/LeggedRobot/src/Ros2Go2Estimator/local_file/new_map.yaml ' 18 | 'params_file:=/home/unitree/ros2_ws/LeggedRobot/src/Ros2Go2Estimator/other/Guide.yaml; ' 19 | 'read -p "Press enter to close"' 20 | ], 21 | output="screen", 22 | ), 23 | ] 24 | ) -------------------------------------------------------------------------------- /sport_control/launch/rviz_launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch_ros.actions import Node 3 | from launch import LaunchDescription 4 | from launch.actions import ExecuteProcess, TimerAction 5 | 6 | def generate_launch_description(): 7 | return LaunchDescription([ 8 | ExecuteProcess( 9 | cmd=[ 10 | "x-terminal-emulator", 11 | "--new-process", 12 | "-e", 13 | 'bash', 14 | '-c', 15 | 'source ~/.bashrc && ' 16 | 'rviz2 -d ~/ros2_ws/LeggedRobot/src/Ros2Go2Estimator/other/SMXFE_odm.rviz;' 17 | 'read -p "Press enter to close"' 18 | ], 19 | output='screen', 20 | ) 21 | ] 22 | ) -------------------------------------------------------------------------------- /sport_control/launch/slam_launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch_ros.actions import Node 3 | from launch import LaunchDescription 4 | from launch.actions import ExecuteProcess, TimerAction # 新增 TimerAction 5 | 6 | def generate_launch_description(): 7 | return LaunchDescription([ 8 | ExecuteProcess( 9 | cmd=[ 10 | "x-terminal-emulator", 11 | "--new-process", 12 | "-e", 13 | 'bash', 14 | '-c', 15 | 'source ~/.bashrc && ' 16 | 'ros2 run slam_toolbox async_slam_toolbox_node --ros-args --params-file ~/ros2_ws/LeggedRobot/src/Ros2Go2Estimator/other/slam_params.yaml & ' 17 | 'read -p "Press enter to close"' 18 | ], 19 | output="screen", 20 | ), 21 | ] 22 | ) -------------------------------------------------------------------------------- /sport_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | sport_control 5 | 0.0.0 6 | TODO: Package description 7 | shine 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | nav2_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | unitree_sdk2 18 | tf2 19 | tf2_ros 20 | launch 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | --------------------------------------------------------------------------------