Merge pull request #436 from JuliaRobotics/21Q2/depr/getmanifolds

away from getManifolds, #244

Project.toml

@@ -31,13 +31,13 @@ TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 
 [compat]
-ApproxManifoldProducts = "0.3.1"
+ApproxManifoldProducts = "0.3.3"
 CoordinateTransformations = "0.5, 0.6, 0.7"
-DistributedFactorGraphs = "0.13"
+DistributedFactorGraphs = "0.14.0"
 Distributions = "0.21, 0.22, 0.23, 0.24"
 DocStringExtensions = "0.7, 0.8"
 FileIO = "1.0.2, 1.1, 1.2"
-IncrementalInference = "0.23.1"
+IncrementalInference = "0.23.2, 0.24"
 JLD2 = "0.2, 0.3, 0.4"
 KernelDensityEstimate = "0.5.1, 0.6"
 Manifolds = "0.4.19"
@@ -48,7 +48,7 @@ Reexport = "0.2, 1.0"
 Requires = "1.0"
 Rotations = "0.12.1, 0.13, 1.0"
 TensorCast = "0.3.3, 0.4"
-TransformUtils = "0.2.7"
+TransformUtils = "0.2.8"
 julia = "1.4"
 
 [extras]

src/Deprecated.jl

@@ -6,19 +6,26 @@
 ##==============================================================================
 
 
+# getManifolds(fctType::InstanceType{Pose2Pose2}) = getManifolds(getDomain(fctType))
+# getManifolds(fctType::InstanceType{Pose2Point2}) = getManifolds(getDomain(fctType))
+# getManifolds(fctType::InstanceType{Point2Point2}) = getManifolds(getDomain(fctType))
+# getManifolds(fctType::InstanceType{Pose2Point2BearingRange}) = getManifolds(getDomain(fctType))
+
+
 ## New Manifold types.  Integration phase towards RoME #244 and AMP #32 / #41
 
 # FIXME, much consolidation required here, see RoME #244
 import IncrementalInference: getManifolds
 
 # TODO consolidate Manifolds typing and objects
-getManifolds(::InstanceType{typeof(AMP.SE2_Manifold)}) = (:Euclid, :Euclid, :Circular)
-getManifolds(::InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid, :Euclid, :Circular, :Euclid, :Euclid)
-getManifolds(::InstanceType{typeof(AMP.SE3_Manifold)}) = (:Euclid, :Euclid, :Euclid, :Circular, :Circular, :Circular)
+# getManifolds(::InstanceType{typeof(AMP.SE2_Manifold)}) = (:Euclid, :Euclid, :Circular)
+# getManifolds(::InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid, :Euclid, :Circular, :Euclid, :Euclid)
+# getManifolds(::InstanceType{typeof(AMP.SE3_Manifold)}) = (:Euclid, :Euclid, :Euclid, :Circular, :Circular, :Circular)
 
 # legacy support, will be deprecated
-Base.convert(::Type{<:Tuple}, mani::InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
-Base.convert(::Type{<:Tuple}, mani::InstanceType{typeof(BearingRange_Manifold)}) = (:Circular,:Euclid)
+Base.convert(::Type{<:Tuple}, ::InstanceType{typeof(AMP.SE2_Manifold)}) = (:Euclid, :Euclid, :Circular)
+Base.convert(::Type{<:Tuple}, ::InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
+Base.convert(::Type{<:Tuple}, ::InstanceType{typeof(BearingRange_Manifold)}) = (:Circular,:Euclid)
 
 # Variables dont need to re-overload these functions from @defVariable (factors dont have easy macro yet)
 # Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Point2}) = AMP.Euclid2

src/RoME.jl

@@ -33,7 +33,9 @@ import TransformUtils: ⊖, ⊕, convert, compare, ominus, veeQuaternion
 import IncrementalInference: convert, getSample, reshapeVec2Mat, DFG, getManifolds
 # not sure why this is gives import error
 import DistributedFactorGraphs: compare
-import DistributedFactorGraphs: getDimension, getManifolds
+import DistributedFactorGraphs: getDimension, getManifold
+import ApproxManifoldProducts: getManifolds # TODO must be deprecated
+
 # const AMP = ApproxManifoldProducts
 
 const InstanceType{T} = Union{Type{<:T},T}

src/TemporaryFunctionality.jl

@@ -1,25 +1,19 @@
 
-import IncrementalInference: selectFactorType, getDomain
+import IncrementalInference: selectFactorType
+# import ApproxManifoldProducts: getManifold
 
-export getDomain
 
 
 ## ============================================================================
 # Starting integration with Manifolds.jl, via ApproxManifoldProducts.jl first
 ## ============================================================================
 
-getDomain(::InstanceType{Point2Point2}) = Point2
-getDomain(::InstanceType{Pose2Point2}) = Point2
-getDomain(::InstanceType{Pose2Pose2}) = Pose2
-# getDomain(::InstanceType{Pose3Point3}) = Point3
-getDomain(::InstanceType{Pose3Pose3}) = Pose3
-getDomain(::InstanceType{Pose2Point2BearingRange}) = BearingRange2
-
-# defines on which variable manifold the measurements for a factor type are represented
-getManifolds(fctType::InstanceType{Pose2Pose2}) = getManifolds(getDomain(fctType))
-getManifolds(fctType::InstanceType{Pose2Point2}) = getManifolds(getDomain(fctType))
-getManifolds(fctType::InstanceType{Point2Point2}) = getManifolds(getDomain(fctType))
-getManifolds(fctType::InstanceType{Pose2Point2BearingRange}) = getManifolds(getDomain(fctType))
+getManifold(::InstanceType{Point2Point2}) = Point2
+getManifold(::InstanceType{Pose2Point2}) = Point2
+getManifold(::InstanceType{Pose2Pose2}) = Pose2
+# getManifold(::InstanceType{Pose3Point3}) = Point3
+getManifold(::InstanceType{Pose3Pose3}) = Pose3
+getManifold(::InstanceType{Pose2Point2BearingRange}) = BearingRange2
 
 
 

src/factors/DynPose2D.jl

@@ -146,8 +146,8 @@ mutable struct DynPose2DynPose2{T <: IIF.SamplableBelief} <: AbstractRelativeRoo
 end
 DynPose2DynPose2(z1::T=MvNormal(zeros(5), diagm([0.01;0.01;0.001;0.1;0.1].^2))) where {T <: IIF.SamplableBelief} = DynPose2DynPose2{T}(z1)
 
-getManifolds(::Type{DynPose2DynPose2}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
-getManifolds(::DynPose2DynPose2) = getManifolds(DynPose2DynPose2)
+# getManifolds(::Type{DynPose2DynPose2}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
+# getManifolds(::DynPose2DynPose2) = getManifolds(DynPose2DynPose2)
 
 
 getSample(cf::CalcFactor{<:DynPose2DynPose2}, N::Int=1) = (rand(cf.factor.Z, N), )

src/factors/Pose2Point2.jl

@@ -11,13 +11,14 @@ Bearing and Range constraint from a Pose2 to Point2 variable.
 """
 struct Pose2Point2{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize
     Zij::T
+    partial::Tuple{Int,Int}
     # empty constructor
-    Pose2Point2{T}() where {T} = new{T}()
-    # regular constructor
-    Pose2Point2{T}(x1::T) where {T <: IIF.SamplableBelief} = new{T}(x1)
+    # Pose2Point2{T}() where {T} = new{T}()
+    # # regular constructor
+    # Pose2Point2{T}(x1::T) where {T <: IIF.SamplableBelief} = new{T}(x1)
 end
 # convenience and default constructor
-Pose2Point2(x1::T=MvNormal(zeros(2),LinearAlgebra.diagm([0.01;0.01]))) where {T <: IIF.SamplableBelief} = Pose2Point2{T}(x1)
+Pose2Point2(x1::T=MvNormal(zeros(2),LinearAlgebra.diagm([0.01;0.01]))) where {T <: IIF.SamplableBelief} = Pose2Point2{T}(x1, (1,2))
 
 # prescribed sampling function
 function getSample(cfo::CalcFactor{<:Pose2Point2}, N::Int=1)
@@ -53,10 +54,10 @@ mutable struct PackedPose2Point2 <: IncrementalInference.PackedInferenceType
 end
 
 function convert(::Type{PackedPose2Point2}, obj::Pose2Point2{T}) where {T <: IIF.SamplableBelief}
-  return PackedPose2Point2(string(obj.Zij))
+  return PackedPose2Point2(convert(PackedSamplableBelief, obj.Zij))
 end
 
 # TODO -- should not be resorting to string, consider specialized code for parametric distribution types and KDEs
 function convert(::Type{Pose2Point2}, packed::PackedPose2Point2)
-  Pose2Point2(extractdistribution(packed.Zij))
+  Pose2Point2(convert(SamplableBelief, packed.Zij))
 end

src/variables/VariableTypes.jl

@@ -8,7 +8,7 @@ $(TYPEDEF)
 
 XY Euclidean manifold variable node softtype.
 """
-@defVariable Point2 Euclidean(2) # 2 (:Euclid, :Euclid)
+@defVariable Point2 Euclidean(2)
 
 
 """
@@ -22,15 +22,15 @@ Example
 p3 = Point3()
 ```
 """
-@defVariable Point3 Euclidean(3) # 3 (:Euclid,:Euclid,:Euclid)
+@defVariable Point3 Euclidean(3)
 
 
 """
 $(TYPEDEF)
 
 Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM.
 """
-@defVariable Pose2 SpecialEuclidean(2) # 3 (:Euclid,:Euclid,:Circular)
+@defVariable Pose2 SpecialEuclidean(2)
 
 """
 $(TYPEDEF)
@@ -42,15 +42,15 @@ Future:
 - Work in progress on AMP3D for proper non-Euler angle on-manifold operations.
 - TODO the AMP upgrade is aimed at resolving 3D to Quat/SE3/SP3 -- current Euler angles will be replaced
 """
-@defVariable Pose3 SpecialEuclidean(3) # 6 (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular)
+@defVariable Pose3 SpecialEuclidean(3)
 
 """
 $(TYPEDEF)
 
 Dynamic point in 2D space with velocity components: `x, y, dx/dt, dy/dt`
 
 """
-@defVariable DynPoint2 Euclidean(4) # 4 (:Euclid,:Euclid,:Euclid,:Euclid)
+@defVariable DynPoint2 Euclidean(4)
 
 """
 $(TYPEDEF)
@@ -60,7 +60,7 @@ Dynamic pose variable with velocity components: `x, y, theta, dx/dt, dy/dt`
 Note
 - The `SE2E2_Manifold` definition used currently is a hack to simplify the transition to Manifolds.jl, see #244 
 """
-@defVariable DynPose2 SE2E2_Manifold # 5 (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
+@defVariable DynPose2 SE2E2_Manifold
 
 
 
@@ -78,7 +78,7 @@ projectCartesian(pose::Union{<:Point2,<:Point3,<:Pose2,<:Pose3,<:DynPoint2,<:Dyn
 
 # Still experimental
 # export BearingRange2
-@defVariable BearingRange2 BearingRange_Manifold # 2 (:Circular, :Euclid)
+@defVariable BearingRange2 BearingRange_Manifold
 
 
 

src/variables/deprecated/DynPoint2D.jl

@@ -12,4 +12,4 @@ mutable struct DynPoint2 <: IncrementalInference.InferenceVariable
   DynPoint2(;ut::Int64=-9999999999) = new(ut, 4,(:Euclid,:Euclid,:Euclid,:Euclid,))
 end
 getDimension(::DynPoint2) = 4
-getManifolds(::DynPoint2) = (:Euclid,:Euclid,:Euclid,:Euclid)
+# getManifolds(::DynPoint2) = (:Euclid,:Euclid,:Euclid,:Euclid)

src/variables/deprecated/Point2D.jl

@@ -6,5 +6,5 @@ XY Euclidean manifold variable node softtype.
 """
 struct Point2 <: IncrementalInference.InferenceVariable end
 getDimension(::Point2) = 2
-getManifolds(::Point2) = (:Euclid, :Euclid)
+# getManifolds(::Point2) = (:Euclid, :Euclid)
 

test/testBasicPose2Stationary.jl

@@ -27,7 +27,7 @@ badval = 0.01.*randn(3,100)
 badval[1,:] .-= 5.0
 badval[2,:] .-= 2.0
 badval[3,:] .+= 0.5
-setValKDE!(fg, :x2, manikde!(badval, getManifolds(Pose2())))
+setValKDE!(fg, :x2, manikde!(badval, Pose2))
 
 N = 100
 # batchSolve!(fg, N=N)

test/testInflation380.jl

@@ -191,7 +191,7 @@ addFactor!(fg, [:x2; :l2], p2br)
 # nonparametric solution
 solveGraph!(fg);
 # parametric solution
-IIF.solveFactorGraphParametric!(fg)
+IIF.solveGraphParametric!(fg)
 
 
 ##
@@ -216,7 +216,7 @@ test_err[1:2] = getPPE(fg, :x2, :default).suggested[1:2] - getPPE(fg, :x2, :para
 
 @test isapprox(test_err[1], 0, atol=0.5)
 @test isapprox(test_err[2], 0, atol=0.5)
-@test isapprox(test_err[3], 0, atol=0.5)
+@test isapprox(test_err[3], 0, atol=0.6)
 
 
 ##

test/testManifoldsPose2Equivalent.jl

@@ -27,30 +27,34 @@ pt = Pose2()
 pts1a = 0.1*randn(1,100)
 pts1b = 0.1*randn(1,100)
 pts1c = TransformUtils.wrapRad.( 0.1*randn(1,100) )
-p = manikde!([pts1a;pts1b;pts1c], getManifolds(Pose2()) )
+p = manikde!([pts1a;pts1b;pts1c], Pose2 )
+# p = manikde!([pts1a;pts1b;pts1c], AMP.getManifolds(Pose2()) )
 
 
 #
 
 pts2a = 3.0*randn(1,100).+4.0
 pts2b = 3.0*randn(1,100)
 pts2c = TransformUtils.wrapRad.(0.15*randn(1,100).+0.7pi)
-q = manikde!([pts2a;pts2b;pts2c], getManifolds(Pose2())  )
+q = manikde!([pts2a;pts2b;pts2c], Pose2  )
+# q = manikde!([pts2a;pts2b;pts2c], AMP.getManifolds(Pose2())  )
 
 
 
 
 pts3a = 3.0*randn(1,100).-4.0
 pts3b = 3.0*randn(1,100)
 pts3c = TransformUtils.wrapRad.(0.15*randn(1,100).-0.7pi)
-r = manikde!([pts3a;pts3b;pts3c], getManifolds(Pose2())  )
+r = manikde!([pts3a;pts3b;pts3c], Pose2  )
+# r = manikde!([pts3a;pts3b;pts3c], AMP.getManifolds(Pose2())  )
 
 
 
 
 # pl = plotPose(pt, [q; r], levels=1)
 
-qr = manifoldProduct([q;r], getManifolds(pt), Niter=3)
+qr = manifoldProduct([q;r], getManifold(pt), Niter=3)
+# qr = manifoldProduct([q;r], AMP.getManifolds(pt), Niter=3)
 
 # pl = plotPose(pt, [q; r; qr], levels=2, c=["cyan";"cyan";"red"])
 
@@ -64,7 +68,7 @@ qr = manifoldProduct([q;r], getManifolds(pt), Niter=3)
 # pts1a = 0.1*randn(1,100)
 # pts1b = 0.1*randn(1,100)
 # pts1c = TransformUtils.wrapRad.( 0.1*randn(1,100) )
-# p = manikde!([pts1a;pts1b;pts1c], getManifolds(Pose2()) )
+# p = manikde!([pts1a;pts1b;pts1c], AMP.getManifolds(Pose2()) )
 
 
 R = [1 1; -1 1]./sqrt(2)
@@ -76,7 +80,8 @@ pts1 = rand(mvn, 100)
 pts2a = pts1[1:1,:].+5.0
 pts2b = pts1[2:2,:].+5.0
 pts2c = TransformUtils.wrapRad.(0.25*randn(1,100).+pi/2)
-q = manikde!([pts2a;pts2b;pts2c], getManifolds(Pose2())  )
+q = manikde!([pts2a;pts2b;pts2c], Pose2  )
+# q = manikde!([pts2a;pts2b;pts2c], AMP.getManifolds(Pose2())  )
 
 
 R = [1 -1; 1 1]./sqrt(2)
@@ -88,12 +93,13 @@ pts2 = rand(mvn, 100)
 pts3a = pts2[1:1,:]
 pts3b = pts2[2:2,:]
 pts3c = TransformUtils.wrapRad.(0.25*randn(1,100).-pi/2)
-r = manikde!([pts3a;pts3b;pts3c], getManifolds(Pose2())  )
+r = manikde!([pts3a;pts3b;pts3c], Pose2  )
+# r = manikde!([pts3a;pts3b;pts3c], AMP.getManifolds(Pose2())  )
 
 
 ##
 
-qr = manifoldProduct([q;r], getManifolds(pt), Niter=8)
+qr = manifoldProduct([q;r], getManifold(pt), Niter=8)
 
 # pl = plotPose(pt, [q; r; qr], levels=3, c=["cyan";"cyan";"red"])
 
@@ -105,23 +111,26 @@ qr = manifoldProduct([q;r], getManifolds(pt), Niter=8)
 pts1a = 0.5*randn(1,100)
 pts1b = 0.5*randn(1,100)
 pts1c = TransformUtils.wrapRad.( 0.1*randn(1,100) )
-p = manikde!([pts1a;pts1b;pts1c], getManifolds(Pose2()) )
+p = manikde!([pts1a;pts1b;pts1c], Pose2 )
+# p = manikde!([pts1a;pts1b;pts1c], AMP.getManifolds(Pose2()) )
 
 
 pts2a = 0.5*randn(1,100)
 pts2b = 1.0*randn(1,100)
 pts2c = TransformUtils.wrapRad.(0.15*randn(1,100).+2pi/3)
-q = manikde!([pts2a;pts2b;pts2c], getManifolds(Pose2())  )
+q = manikde!([pts2a;pts2b;pts2c], Pose2  )
+# q = manikde!([pts2a;pts2b;pts2c], AMP.getManifolds(Pose2())  )
 
 
 pts3a = 1.0*randn(1,100)
 pts3b = 0.5*randn(1,100)
 pts3c = TransformUtils.wrapRad.(0.15*randn(1,100).-2pi/3)
-r = manikde!([pts3a;pts3b;pts3c], getManifolds(Pose2())  )
+r = manikde!([pts3a;pts3b;pts3c], Pose2  )
+# r = manikde!([pts3a;pts3b;pts3c], AMP.getManifolds(Pose2())  )
 
 
 
-pqr = manifoldProduct([p;q;r], getManifolds(pt))
+pqr = manifoldProduct([p;q;r], getManifold(pt))
 
 # pl = plotPose(pt, [p; q; r; pqr], levels=1, c=["cyan";"cyan";"cyan";"red"])
 
@@ -153,13 +162,15 @@ N = 100
 pts2a = 3.0*randn(1,N)
 pts2b = 3.0*randn(1,N)
 pts2c = TransformUtils.wrapRad.(0.15*randn(1,N).+0.9pi)
-q = manikde!([pts2a;pts2b;pts2c], getManifolds(Pose2())  )
+q = manikde!([pts2a;pts2b;pts2c], Pose2  )
+# q = manikde!([pts2a;pts2b;pts2c], AMP.getManifolds(Pose2())  )
 
 
 pts3a = 3.0*randn(1,N).+4.0
 pts3b = 3.0*randn(1,N).+4.0
 pts3c = TransformUtils.wrapRad.(0.15*randn(1,N).-0.9pi)
-r = manikde!([pts3a;pts3b;pts3c], getManifolds(Pose2())  )
+r = manikde!([pts3a;pts3b;pts3c], Pose2  )
+# r = manikde!([pts3a;pts3b;pts3c], AMP.getManifolds(Pose2())  )
 
 
 fg = initfg()