diff --git a/src/entities/DFGFactor.jl b/src/entities/DFGFactor.jl index 8b4b66f9..37d7d44c 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/DFGFactor.jl @@ -2,7 +2,6 @@ ## Abstract Types ##============================================================================== -# TODO consider changing this to AbstractFactor abstract type AbstractFactor end abstract type AbstractPackedFactor end diff --git a/src/services/DFGFactor.jl b/src/services/DFGFactor.jl index 9629720e..7d2574b0 100644 --- a/src/services/DFGFactor.jl +++ b/src/services/DFGFactor.jl @@ -52,6 +52,59 @@ function _getPriorType(_type::Type{<:InferenceVariable}) return getfield(_type.name.module, Symbol(:Prior, _type.name.name)) end +##============================================================================== +## Default Factors Function Macro +##============================================================================== +export PackedSamplableBelief +# export pack, unpack, packDistribution, unpackDistribution + +function pack end +function unpack end +function packDistribution end +function unpackDistribution end + +abstract type PackedSamplableBelief end +StructTypes.StructType(::Type{<:PackedSamplableBelief}) = StructTypes.UnorderedStruct() + +""" + @defFactorType StructName factortype<:AbstractFactor manifolds<:ManifoldsBase.AbstractManifold + +A macro to create a new factor function with name `StructName` and manifolds. Note that +the `manifolds` is an object and *must* be a subtype of `ManifoldsBase.AbstractManifold`. +See documentation in [Manifolds.jl on making your own](https://juliamanifolds.github.io/Manifolds.jl/stable/examples/manifold.html). + +Example: +``` +DFG.@defFactorType Pose2Pos2 AbstractManifoldMinimize SpecialEuclidean(2) +``` +""" +macro defFactorType(structname, factortype, manifold) + packedstructname = Symbol("Packed", structname) + return esc( + quote + Base.@__doc__ struct $structname{T} <: $factortype + Z::T + end + + Base.@__doc__ struct $packedstructname{T <: PackedSamplableBelief} <: + AbstractPackedFactor + Z::T + end + + # user manifold must be a <:Manifold + @assert ($manifold isa AbstractManifold) "@defFactorType of " * + string($structname) * + " requires that the " * + string($manifold) * + " be a subtype of `ManifoldsBase.AbstractManifold`" + + DFG.getManifold(::Type{$structname}) = $manifold + DFG.pack(d::$structname) = $packedstructname(DFG.packDistribution(d.Z)) + DFG.unpack(d::$packedstructname) = $structname(DFG.unpackDistribution(d.Z)) + end, + ) +end + ##============================================================================== ## Factors ##==============================================================================