defFactorType macro (#1077)

* defFactorFunction macro

* Apply suggestions from code review

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* update defFactorType

* Apply suggestions from code review

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* Update DFGFactor.jl

---------

Co-authored-by: Johannes Terblanche <Affie@users.noreply.github.com>
Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

Author: Affie

src/entities/DFGFactor.jl

@@ -2,7 +2,6 @@
 ## Abstract Types
 ##==============================================================================
 
-# TODO consider changing this to AbstractFactor
 abstract type AbstractFactor end
 abstract type AbstractPackedFactor end
 

src/services/DFGFactor.jl

@@ -64,6 +64,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
 ##==============================================================================