Merge pull request #1299 from JuliaRobotics/21Q2/enh/vartypeInOptim

varType in _solveLambdaNumeric and translation and broken SE2 tests

src/NumericalCalculations.jl

@@ -78,14 +78,15 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
                               objResX::Function,
                               residual::AbstractVector{<:Real},
                               u0,#::AbstractVector{<:Real},
-                              islen1::Bool=false  )  where {N_,F<:AbstractManifoldMinimize,S,T}
-                              # retries::Int=3 )
+                              variableType::InferenceVariable,  
+                              islen1::Bool=false)  where {N_,F<:AbstractManifoldMinimize,S,T}
   #
-  M = fcttype.M
+  M = getManifold(variableType)#fcttype.M
   # the variable is a manifold point, we are working on the tangent plane in optim for now.
   # 
-  #TODO this is not correct we need the point identity also
-  ϵ = identity(M, u0)
+  #TODO this is not general to all manifolds, should work for lie groups.
+  # ϵ = identity(M, u0)
+  ϵ = getPointIdentity(variableType)
   # X0c = get_coordinates(M, u0, log(M, ϵ, u0), DefaultOrthogonalBasis()) 
   X0c = vee(M, u0, log(M, ϵ, u0)) 
 
@@ -296,7 +297,8 @@ function _solveCCWNumeric!( ccwl::Union{CommonConvWrapper{F},
 
   # do the parameter search over defined decision variables using Minimization
   # retval = _solveLambdaNumeric(getFactorType(ccwl), _hypoObj, cpt_.res, cpt_.X[smpid][cpt_.p], islen1 )
-  retval = _solveLambdaNumeric(getFactorType(ccwl), _hypoObj, cpt_.res, cpt_.X[smpid], islen1 )
+  sfidx = ccwl.varidx
+  retval = _solveLambdaNumeric(getFactorType(ccwl), _hypoObj, cpt_.res, cpt_.X[smpid], ccwl.vartypes[sfidx](), islen1)
 
   # Check for NaNs
   if sum(isnan.(retval)) != 0

test/testSpecialEuclidean2Mani.jl

@@ -0,0 +1,54 @@
+using DistributedFactorGraphs
+using IncrementalInference
+using Manifolds
+using StaticArrays
+using Test
+
+@testset "Test SpecialEuclidean(2)" begin
+
+Base.convert(::Type{<:Tuple}, M::SpecialEuclidean{2}) = (:Euclid, :Euclid, :Circular)
+Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{SpecialEuclidean{2}})  = (:Euclid, :Euclid, :Circular)
+
+# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
+@defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])
+
+M = getManifold(SpecialEuclidean2)
+@test M == SpecialEuclidean(2)
+pT = getPointType(SpecialEuclidean2)
+@test pT == ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}
+# @test pT == ProductRepr{Tuple{MVector{2, Float64}, MMatrix{2, 2, Float64, 4}}}
+pϵ = getPointIdentity(SpecialEuclidean2)
+# @test_broken pϵ == ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
+@test_broken pϵ == ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])
+
+@test is_point(getManifold(SpecialEuclidean2), getPointIdentity(SpecialEuclidean2))
+
+##
+fg = initfg()
+
+v0 = addVariable!(fg, :x0, SpecialEuclidean2)
+
+# mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
+mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
+p = addFactor!(fg, [:x0], mp)
+
+@test_broken doautoinit!(fg, :x0)
+
+##
+vnd = getVariableSolverData(fg, :x0)
+@test all(isapprox.(mean(vnd.val), ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0]), atol=0.1).parts)
+@test all(is_point.(Ref(M), vnd.val))
+
+##
+v1 = addVariable!(fg, :x1, SpecialEuclidean2)
+mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([pi], [0.01]))
+f = addFactor!(fg, [:x0, :x1], mf)
+
+@test_broken doautoinit!(fg, :x1)
+
+##
+smtasks = Task[]
+@test_broken solveTree!(fg; smtasks, verbose=true, recordcliqs=ls(fg))
+# hists = fetchCliqHistoryAll!(smtasks);
+
+end

test/testSpecialOrthogonalMani.jl

@@ -99,5 +99,14 @@ vnd = getVariableSolverData(fg, :x1)
 smtasks = Task[]
 solveTree!(fg; smtasks, verbose=true, recordcliqs=ls(fg))
 
+# test them again after solve
+vnd = getVariableSolverData(fg, :x0)
+@test all(isapprox.( mean(SpecialOrthogonal(3),vnd.val), [1 0 0; 0 1 0; 0 0 1], atol=0.01))
+@test all(is_point.(Ref(M), vnd.val))
+
+vnd = getVariableSolverData(fg, :x1)
+@test all(isapprox.( mean(SpecialOrthogonal(3),vnd.val), [0.9999 -0.00995 0.01005; 0.01005 0.9999 -0.00995; -0.00995 0.01005 0.9999], atol=0.01))
+@test all(is_point.(Ref(M), vnd.val))
+
 
 end

examples/dev/testManiFactors.jl → test/testTranslationMani.jl

@@ -1,8 +1,10 @@
 using DistributedFactorGraphs
 using IncrementalInference
-using StaticArrays
 using Manifolds
+using StaticArrays
+using Test
 
+@testset "Test TranslationGroup(2)" begin
 ## ======================================================================================
 ## 
 ## ======================================================================================
@@ -26,11 +28,9 @@ p = addFactor!(fg, [:x0], mp;  graphinit=true)
 
 doautoinit!(fg, :x0)
 
-# @enter addFactor!(fg, [:x0], mp;  graphinit=true)
-
-
 mf = ManifoldFactor(TranslationGroup(2), MvNormal([1.0, 2.0], [0.1,0.1]))
 f = addFactor!(fg, [:x0, :x1], mf)
 
-
 solveGraph!(fg)
+
+end