Merge pull request #743 from JuliaRobotics/21Q1/refac/defvarmani

switch defVariable to Manifolds.jl

Author: dehann

Project.toml

@@ -1,6 +1,6 @@
 name = "DistributedFactorGraphs"
 uuid = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
-version = "0.12.1"
+version = "0.13.0"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
@@ -12,6 +12,7 @@ JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6"
 JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3"
 LightGraphs = "093fc24a-ae57-5d10-9952-331d41423f4d"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb"
 Neo4j = "d2adbeaf-5838-5367-8a2f-e46d570981db"
 Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
@@ -29,6 +30,7 @@ DocStringExtensions = "0.8, 0.9, 0.10, 1"
 JSON = "0.21"
 JSON2 = "0.3.1"
 LightGraphs = "1.2, 1.3"
+ManifoldsBase = "0.10"
 Neo4j = "2"
 Pkg = "1.4, 1.5"
 Reexport = "0.2, 0.3, 0.4, 0.5, 1"
@@ -39,8 +41,9 @@ julia = "1.4"
 
 [extras]
 GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231"
+Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "GraphPlot", "Pkg"]
+test = ["Test", "GraphPlot", "Manifolds", "Pkg"]

src/DistributedFactorGraphs.jl

@@ -29,6 +29,11 @@ using SparseArrays
 using UUIDs
 using Pkg
 
+# used for @defVariable
+import ManifoldsBase
+import ManifoldsBase: Manifold, manifold_dimension
+export Manifold, manifold_dimension
+
 import Base: getindex
 
 
@@ -130,7 +135,7 @@ export getSolverData
 export getVariableType
 
 # VariableType functions
-export getDimension, getManifolds
+export getDimension, getManifolds, getManifold
 
 # Small Data CRUD
 export SmallDataTypes, getSmallData, addSmallData!, updateSmallData!, deleteSmallData!, listSmallData, emptySmallData!

src/services/DFGVariable.jl

@@ -63,28 +63,56 @@ getVariableType(dfg::AbstractDFG, lbl::Symbol) = getVariableType(getVariable(dfg
 Interface function to return the `variableType` dimension of an InferenceVariable, extend this function for all Types<:InferenceVariable.
 """
 function getDimension end
+"""
+    $SIGNATURES
+Interface function to return the `<:ManifoldsBase.Manifold` object of `variableType<:InferenceVariable`, extend this function for all `Types<:InferenceVariable`.
+"""
+function getManifold end
+
 """
     $SIGNATURES
 Interface function to return the `variableType` manifolds of an InferenceVariable, extend this function for all Types<:InferenceVariable.
 """
 function getManifolds end
 
+getManifolds(::Type{<:T}) where {T <: ManifoldsBase.Manifold} = convert(Tuple, T)
+getManifolds(::T) where {T <: ManifoldsBase.Manifold} = getManifolds(T)
+
+getDimension(t_::T) where {T <: ManifoldsBase.Manifold} = manifold_dimension(t_)
+
 """
-   @defVariable StructName dimension manifolds
-A macro to create a new variable with name `StructName`, dimension and manifolds.
+    @defVariable StructName manifolds<:ManifoldsBase.Manifold
+
+A macro to create a new variable with name `StructName` and manifolds.  Note that 
+the `manifolds` is an object and *must* be a subtype of `ManifoldsBase.Manifold`.
+See documentation in [Manifolds.jl on making your own](https://juliamanifolds.github.io/Manifolds.jl/stable/examples/manifold.html). 
+
 Example:
 ```
-DFG.@defVariable Pose2 3 (:Euclid, :Euclid, :Circular)
+DFG.@defVariable Pose2 SpecialEuclidean(2)
 ```
 """
-macro defVariable(structname, dimension::Int, manifolds)#::Vararg{Symbol})#NTuple{dimension, Symbol})
-    # :(struct $structname <: InferenceVariable end)
+macro defVariable(structname, manifold)
     return esc(quote
         Base.@__doc__ struct $structname <: InferenceVariable end
-        DistributedFactorGraphs.getDimension(::$structname) = $dimension
-        DistributedFactorGraphs.getManifolds(::$structname) = $manifolds
+
+        @assert ($manifold isa Manifold) "@defVariable of "*string($structname)*" requires that the "*string($manifold)*" be a subtype of `ManifoldsBase.Manifold`"
+
+        # user manifold must be a <:Manifold
+        Base.convert(::Type{<:Manifold}, ::Union{<:T, Type{<:T}}) where {T <: $structname} = $manifold 
+
+        DFG.getManifold(::Type{M}) where {M <: $structname} = $manifold
+        DFG.getManifold(::M) where {M <: $structname} = getManifold(M)
+        
+        DFG.getDimension(::Type{M}) where {M <: $structname} = manifold_dimension(getManifold(M))
+        DFG.getDimension(::M) where {M <: $structname} = manifold_dimension(getManifold(M))
+
+        # # FIXME legacy API to be deprecated
+        DFG.getManifolds(::Type{M}) where {M <: $structname} = convert(Tuple, $manifold)
+        DFG.getManifolds(::M) where {M <: $structname} = convert(Tuple, $manifold)
     end)
 end
+
 ##------------------------------------------------------------------------------
 ## solvedCount
 ##------------------------------------------------------------------------------

test/runtests.jl

@@ -21,6 +21,10 @@ using UUIDs
 # logger = SimpleLogger(stdout, Logging.Debug)
 # global_logger(logger)
 
+@testset "Check @defVariable design" begin
+    include("test_defVariable.jl")
+end
+
 include("testBlocks.jl")
 
 @testset "Test generated ==" begin

test/testBlocks.jl

@@ -1,6 +1,7 @@
 using DistributedFactorGraphs
 using Test
 using Dates
+using Manifolds
 
 import Base: convert
 
@@ -11,13 +12,18 @@ import Base: convert
 #     TestVariableType1() = new(1,(:Euclid,))
 # end
 
-DFG.@defVariable TestVariableType1 1 (:Euclid,)
 
-struct TestVariableType2 <: InferenceVariable
-    dims::Int
-    manifolds::Tuple{Symbol, Symbol}
-    TestVariableType2() = new(2,(:Euclid,:Circular,))
-end
+Base.convert(::Type{<:Tuple}, ::typeof(Euclidean(1))) = (:Euclid,)
+Base.convert(::Type{<:Tuple}, ::typeof(Euclidean(2))) = (:Euclid, :Euclid)
+
+@defVariable TestVariableType1 Euclidean(1)
+@defVariable TestVariableType2 Euclidean(2)
+
+# struct TestVariableType2 <: InferenceVariable
+#     dims::Int
+#     manifolds::Tuple{Symbol, Symbol}
+#     TestVariableType2() = new(2,(:Euclid,:Circular,))
+# end
 
 
 struct TestFunctorInferenceType1 <: AbstractRelative end

test/test_defVariable.jl

@@ -0,0 +1,81 @@
+
+using ManifoldsBase, Manifolds
+using Test
+
+# abstract type InferenceVariable end
+
+##
+
+Base.convert(::Type{<:Tuple}, ::typeof(Euclidean(1))) = (:Euclid,)
+Base.convert(::Type{<:Tuple}, ::typeof(Euclidean(2))) = (:Euclid, :Euclid)
+
+##
+
+macro defVariable(structname, manifold)
+  return esc(quote
+      Base.@__doc__ struct $structname <: InferenceVariable end
+
+      @assert ($manifold isa Manifold) "@defVariable of "*string($structname)*" requires that the "*string($manifold)*" be a subtype of `ManifoldsBase.Manifold`"
+
+      # manifold must be is a <:Manifold
+      Base.convert(::Type{<:Manifold}, ::Union{<:T, Type{<:T}}) where {T <: $structname} = $manifold 
+
+      getManifold(::Type{M}) where {M <: $structname} = $manifold
+      getManifold(::M) where {M <: $structname} = getManifold(M)
+      
+      getDimension(::Type{M}) where {M <: $structname} = manifold_dimension(getManifold(M))
+      getDimension(::M) where {M <: $structname} = manifold_dimension(getManifold(M))
+      # FIXME legacy API to be deprecated
+      getManifolds(::Type{M}) where {M <: $structname} = convert(Tuple, $manifold)
+      getManifolds(::M) where {M <: $structname} = convert(Tuple, $manifold)
+  end)
+end
+
+
+##
+
+
+ex = macroexpand(Main, :(@defVariable(TestVariableType1, Euclidean(1))) )
+
+
+##
+
+struct NotAManifold end
+
+try
+  @defVariable(MyVar, NotAManifold())
+catch AssertionError
+  @test true
+end
+
+##
+
+@defVariable(TestVariableType1, Euclidean(1))
+@defVariable(TestVariableType2, Euclidean(2))
+
+
+##
+
+@test getManifold( TestVariableType1) == Euclidean(1)
+@test getManifold( TestVariableType2) == Euclidean(2)
+
+# legacy
+@test getManifolds(TestVariableType1) == (:Euclid,)
+@test getDimension(TestVariableType1) === 1
+@test getManifolds(TestVariableType2) == (:Euclid,:Euclid)
+@test getDimension(TestVariableType2) === 2
+
+##
+
+
+@test getManifold( TestVariableType1()) == Euclidean(1)
+@test getManifold( TestVariableType2()) == Euclidean(2)
+
+# legacy
+@test getManifolds(TestVariableType1()) == (:Euclid,)
+@test getDimension(TestVariableType1()) === 1
+@test getManifolds(TestVariableType2()) == (:Euclid,:Euclid)
+@test getDimension(TestVariableType2()) === 2
+
+
+##