WIP on SE2 test

Author: Affie

IncrementalInference/Project.toml

@@ -90,9 +90,9 @@ KernelDensityEstimate = "0.5.6"
 LieGroups = "0.1.1"
 LinearAlgebra = "1.10"
 ManifoldDiff = "0.3, 0.4"
-Manifolds = "=0.10.16"
+Manifolds = "0.10.17"
 ManifoldsBase = "0.15, 1"
-Manopt = "^0.5.14"
+Manopt = "0.5.14"
 MetaGraphs = "0.7, 0.8"
 Optim = "1"
 OrderedCollections = "1"

IncrementalInference/src/ExportAPI.jl

@@ -21,7 +21,6 @@ export AbstractDFG,
   setSolvedCount!,
   listSupersolves,
   listSolveKeys,
-  cloneSolveKey!,
   diagm,
   listBlobEntries,
   FolderStore,
@@ -106,8 +105,6 @@ export CSMHistory,
   joinLogPath,
   lsfPriors,
   isPrior,
-  lsTypes,
-  lsfTypes,
   findClosestTimestamp,
   printVariable,
   printFactor,

IncrementalInference/src/Factors/Circular.jl

@@ -4,7 +4,7 @@
 
 function (cf::CalcFactor{<:CircularCircular})(X, p, q)
   M = getManifold(cf)
-  return distanceTangent2Point(M, X, p, q)
+  return measurement_residual(M, X, p, q)
 end
 
 function getSample(cf::CalcFactor{<:CircularCircular})

IncrementalInference/src/Factors/GenericFunctions.jl

@@ -13,8 +13,8 @@ function DFG.getDimension(Z::FluxModelsDistribution)
     Z.outputDim[1]
   else
     error(
-    "can only do single index tensor at this time, please open an issue with Caesar.jl",
-  )
+      "can only do single index tensor at this time, please open an issue with Caesar.jl",
+    )
   end
 end
 DFG.getDimension(Z::ManifoldKernelDensity) = getManifold(Z) |> getDimension
@@ -24,31 +24,21 @@ DFG.getDimension(Z::BallTreeDensity) = Ndim(Z)
 ## ======================================================================================
 ## Generic manifold cost functions
 ## ======================================================================================
-"""
-    $SIGNATURES
-Generic function that can be used in binary factors to calculate distance between points on Lie Groups with measurements.
-"""
-function distancePoint2Point(M::SemidirectProductGroup, m, p, q)
-  q̂ = Manifolds.compose(M, p, m)
-  # return log(M, q, q̂)
-  return vee(M, q, log(M, q, q̂))
-  # return distance(M, q, q̂)
-end
-
-#::MeasurementOnTangent
-function distanceTangent2Point(M::SemidirectProductGroup, X, p, q)
-  q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X)) #for groups
-  # return log(M, q, q̂)
-  return vee(M, q, log(M, q, q̂))
-  # return distance(M, q, q̂)
-end
+# """
+#     $SIGNATURES
+# Generic function that can be used in binary factors to calculate distance between points on Lie Groups with measurements.
+# """
+# function distancePoint2Point(M::SemidirectProductGroup, m, p, q)
+#   q̂ = Manifolds.compose(M, p, m)
+#   # return log(M, q, q̂)
+#   return vee(M, q, log(M, q, q̂))
+#   # return distance(M, q, q̂)
+# end
 
 # ::MeasurementOnTangent
-function distanceTangent2Point(M::AbstractManifold, X, p, q)
-  q̂ = exp(M, p, X)
-  # return log(M, q, q̂)
-  return vee(M, q, log(M, q, q̂))
-  # return distance(M, q, q̂)
+function measurement_residual(G::LieGroup, X, p, q)
+  X̂ = log(G, p, q)
+  return vee(LieAlgebra(G), p, X - X̂) # TODO check sign
 end
 
 """
@@ -69,10 +59,7 @@ export ManifoldFactor
 # For now, `Z` is on the tangent space in coordinates at the point used in the factor.
 # For groups just the lie algebra
 # As transition it will be easier this way, we can reevaluate
-struct ManifoldFactor{
-  M <: AbstractManifold, 
-  T <: SamplableBelief
-} <: RelativeObservation
+struct ManifoldFactor{M <: AbstractManifold, T <: SamplableBelief} <: RelativeObservation
   M::M
   Z::T
 end
@@ -96,15 +83,13 @@ end
 
 # function (cf::CalcFactor{<:ManifoldFactor{<:AbstractDecoratorManifold}})(Xc, p, q)
 function (cf::CalcFactor{<:ManifoldFactor})(X, p, q)
-  return distanceTangent2Point(cf.factor.M, X, p, q)
+  return measurement_residual(cf.factor.M, X, p, q)
 end
 
-
 ## ======================================================================================
 ## adjoint factor - adjoint action applied to the measurement
 ## ======================================================================================
 
-
 # Adjoints defined in ApproxManifoldProducts
 struct AdFactor{F <: RelativeObservation} <: RelativeObservation
   factor::F
@@ -147,7 +132,7 @@ end
 getMeasurementParametric(f::AdFactor) = getMeasurementParametric(f.factor)
 
 getManifold(f::AdFactor) = getManifold(f.factor)
-function getSample(cf::CalcFactor{<:AdFactor}) 
+function getSample(cf::CalcFactor{<:AdFactor})
   M = getManifold(cf)
   return sampleTangent(M, cf.factor.factor.Z)
 end
@@ -168,8 +153,8 @@ struct ManifoldPrior{M <: AbstractManifold, T <: SamplableBelief, P, B <: Abstra
   retract_method::AbstractRetractionMethod
 end
 
-function ManifoldPrior(M::AbstractDecoratorManifold, p, Z)
-  return ManifoldPrior(M, p, Z, ManifoldsBase.VeeOrthogonalBasis(), ExponentialRetraction())
+function ManifoldPrior(M::LieGroup, p, Z)
+  return ManifoldPrior(M, p, Z, MB.VeeOrthogonalBasis(), MB.ExponentialRetraction())
 end
 
 DFG.getManifold(f::ManifoldPrior) = f.M
@@ -199,7 +184,7 @@ function getFactorMeasurementParametric(fac::ManifoldPrior)
   dims = manifold_dimension(M)
   meas = fac.p
   iΣ = convert(SMatrix{dims, dims}, invcov(fac.Z))
-  meas, iΣ
+  return meas, iΣ
 end
 
 #TODO investigate SVector if small dims, this is slower

IncrementalInference/src/IncrementalInference.jl

@@ -13,8 +13,15 @@ using Reexport
 # @reexport using Graphs
 @reexport using LinearAlgebra
 
-using Manifolds
-using LieGroups
+import Manifolds
+using Manifolds: ProductGroup, AbstractDecoratorManifold, get_vector, get_coordinates, ProductManifold, PowerManifold, GeodesicInterpolation, DefaultOrthogonalBasis, get_vector!
+using ManifoldsBase
+using ManifoldsBase: ℝ, AbstractManifold, AbstractBasis, TypeParameter, AbstractRetractionMethod, AbstractPowerManifold, NestedReplacingPowerRepresentation, retract, ExponentialRetraction
+
+import LieGroups
+using LieGroups: LieGroup, LieAlgebra, ProductLieGroup, hat, vee, compose, AbstractProductGroupOperation, AdditionGroupOperation, SpecialEuclideanGroup, SpecialOrthogonalGroup
+# using LieGroups: ProductGroupOperation, SemiDirectProductGroupOperation
+
 using RecursiveArrayTools: ArrayPartition
 export ArrayPartition
 using ManifoldDiff
@@ -45,8 +52,6 @@ using StructTypes
 
 using StaticArrays
 
-using ManifoldsBase
-using ManifoldsBase: TypeParameter
 # for BayesTree
 using MetaGraphs
 using Logging

IncrementalInference/src/manifolds/services/ManifoldSampling.jl

@@ -28,7 +28,7 @@ function sampleTangent(
   M::AbstractManifold,
   z::Distribution,
   p = getPointIdentity(M),
-  basis::AbstractBasis = DefaultOrthogonalBasis()
+  basis::AbstractBasis = DefaultOrthogonalBasis(),
 )
   return get_vector(M, p, rand(z), basis)
 end
@@ -39,18 +39,20 @@ function sampleTangent(
   p = getPointIdentity(M),
 )
   return hat(M, p, SVector{length(z)}(rand(z))) #TODO make sure all Distribution has length, 
-                                                # if this errors maybe fall back no next line
+  # if this errors maybe fall back no next line
   # return convert(typeof(p), hat(M, p, rand(z, 1)[:])) #TODO find something better than (z,1)[:]
 end
 
-function sampleTangent(
-  M::LieGroup,
-  z::Distribution,
-  p = nothing,
-)
-  return hat(LieAlgebra(M), SVector{length(z)}(rand(z)), ArrayPartition{Float64}) #TODO make sure all Distribution has length, 
-                                                # if this errors maybe fall back no next line
-  # return convert(typeof(p), hat(M, p, rand(z, 1)[:])) #TODO find something better than (z,1)[:]
+function sampleTangent(M::LieGroup, z::Distribution, p = getPointIdentity(M))
+  return hat(LieAlgebra(M), SVector{length(z)}(rand(z)), typeof(p))
+end
+
+function sampleTangent(M::LieGroup, x::ManifoldKernelDensity, p = mean(x))
+  # get legacy matrix of coordinates and selected labels
+  #TODO make sure that when `sample` is replaced in MKD, coordinates is a vector
+  coords, lbls = sample(x.belief, 1)
+  X = hat(LieAlgebra(x.manifold), coords[:], typeof(p))
+  return X
 end
 
 """
@@ -152,7 +154,7 @@ function getSample(cf::CalcFactor{<:AbstractPrior})
 end
 
 function getSample(cf::CalcFactor{<:AbstractRelative})
-  M =getManifold(cf)
+  M = getManifold(cf)
   if hasfield(typeof(cf.factor), :Z)
     X = sampleTangent(M, cf.factor.Z)
   else

IncrementalInference/src/manifolds/services/ManifoldsExtentions.jl

@@ -9,21 +9,20 @@
     
 Adapts Manifolds.jl manifolds for use in Optim.jl
 """
-struct ManifoldWrapper{TM<:AbstractManifold} <: Optim.Manifold
-    M::TM
+struct ManifoldWrapper{TM <: AbstractManifold} <: Optim.Manifold
+  M::TM
 end
 
 function Optim.retract!(M::ManifoldWrapper, x)
-    ManifoldsBase.embed_project!(M.M, x, x)
-    return x
+  ManifoldsBase.embed_project!(M.M, x, x)
+  return x
 end
 
 function Optim.project_tangent!(M::ManifoldWrapper, g, x)
-    ManifoldsBase.embed_project!(M.M, g, x, g)
-    return g
+  ManifoldsBase.embed_project!(M.M, g, x, g)
+  return g
 end
 
-
 ## ================================================================================================
 ## AbstractPowerManifold with N as field to avoid excessive compiling time.
 ## ================================================================================================
@@ -41,7 +40,7 @@ end
 
 function Manifolds.get_vector!(M::NPowerManifold, Y, p, c, B::AbstractBasis)
   dim = manifold_dimension(M.manifold)
-  rep_size = representation_size(M.manifold)
+  rep_size = Manifolds.representation_size(M.manifold)
   v_iter = 1
   for i in Manifolds.get_iterator(M)
     Y[i] = get_vector(
@@ -57,7 +56,7 @@ function Manifolds.get_vector!(M::NPowerManifold, Y, p, c, B::AbstractBasis)
 end
 
 function Manifolds.exp!(M::NPowerManifold, q, p, X)
-  rep_size = representation_size(M.manifold)
+  rep_size = Manifolds.representation_size(M.manifold)
   for i in Manifolds.get_iterator(M)
     q[i] = exp(
       M.manifold,
@@ -105,15 +104,25 @@ function DFG.getPointIdentity(G::ProductGroup, ::Type{T} = Float64) where {T <:
 end
 
 # fallback 
-function DFG.getPointIdentity(G::GroupManifold, ::Type{T} = Float64) where {T <: Real}
+function DFG.getPointIdentity(G::LieGroup, ::Type{T} = Float64) where {T <: Real}
   return error("getPointIdentity not implemented on $G")
 end
 
 function DFG.getPointIdentity(
-  @nospecialize(G::ProductManifold),
+  G::LieGroups.TranslationGroup{ℝ, TypeParameter{Tuple{N}}},
   ::Type{T} = Float64,
-) where {T <: Real}
-  return ArrayPartition(map(x -> getPointIdentity(x, T), G.manifolds))
+) where {N, T <: Real}
+  return zeros(SVector{N, T})
+end
+
+#TODO test
+function DFG.getPointIdentity(
+  PrG::LieGroup{𝔽, Op, M},
+  ::Type{T} = Float64,
+) where {𝔽, Op <: AbstractProductGroupOperation, M <: ProductManifold, T <: Real}
+  PrM = PrG.manifold
+  ε = map(getPointIdentity, map(LieGroup, PrM.manifolds, PrG.op.operations), T)
+  return ArrayPartition(ε)
 end
 
 function DFG.getPointIdentity(
@@ -128,33 +137,25 @@ function DFG.getPointIdentity(M::NPowerManifold, ::Type{T} = Float64) where {T <
   return fill(getPointIdentity(M.manifold, T), M.N)
 end
 
-function DFG.getPointIdentity(G::SemidirectProductGroup, ::Type{T} = Float64) where {T <: Real}
-  M = base_manifold(G)
-  N, H = M.manifolds
-  np = getPointIdentity(N, T)
-  hp = getPointIdentity(H, T)
-  return ArrayPartition(np, hp)
-end
-
 function DFG.getPointIdentity(
-  ::typeof(SpecialEuclideanGroup(2; variant=:right)),
-  ::Type{T} = Float64
-) where T
+  ::typeof(SpecialEuclideanGroup(2; variant = :right)),
+  ::Type{T} = Float64,
+) where {T <: Real}
   N = 2
-  return ArrayPartition(zeros(SVector{N,T}), SMatrix{N, N, T}(I))
+  return ArrayPartition(zeros(SVector{N, T}), SMatrix{N, N, T}(I))
 end
 
 function DFG.getPointIdentity(
-  ::typeof(SpecialEuclideanGroup(3; variant=:right)),
-  ::Type{T} = Float64
-) where T 
+  ::typeof(SpecialEuclideanGroup(3; variant = :right)),
+  ::Type{T} = Float64,
+) where {T}
   N = 3
-  return ArrayPartition(zeros(SVector{N,T}), SMatrix{N, N, T}(I))
+  return ArrayPartition(zeros(SVector{N, T}), SMatrix{N, N, T}(I))
 end
 
 function DFG.getPointIdentity(
-  G::SpecialOrthogonal{TypeParameter{Tuple{N}}},
-  ::Type{T} = Float64
+  G::SpecialOrthogonalGroup{TypeParameter{Tuple{N}}},
+  ::Type{T} = Float64,
 ) where {N, T <: Real}
   return SMatrix{N, N, T}(I)
 end
@@ -163,9 +164,9 @@ function DFG.getPointIdentity(
   G::Manifolds.TranslationGroup{TypeParameter{Tuple{N}}},
   ::Type{T} = Float64,
 ) where {N, T <: Real}
-  return zeros(SVector{N,T})
+  return zeros(SVector{N, T})
 end
 
-function DFG.getPointIdentity(G::RealCircleGroup, ::Type{T} = Float64) where {T <: Real}
+function DFG.getPointIdentity(G::LieGroup{ℝ,AdditionGroupOperation,<:Circle{ℝ}}, ::Type{T} = Float64) where {T <: Real}
   return [zero(T)] #FIXME we cannot support scalars yet
 end

IncrementalInference/src/parametric/services/ParametricManopt.jl

@@ -212,7 +212,7 @@ function (jacF!::JacF_RLM!)(
   cache = jacF!.Jcache
 
   fill!(X0, 0)
-  
+
   # TODO make sure closure performs (let, ::, or (jacF!::JacF_RLM!)(res, Xc))
   function costf!(res, Xc)
     get_vector!(M, X, p, Xc, basis_domain)
@@ -414,7 +414,7 @@ function solve_RLM_conditional(
 
   # get the subgraph formed by all frontals, separators and fully connected factors
   varlabels = union(frontals, separators)
-  faclabels = sortDFG(setdiff(getNeighborhood(fg, varlabels, 1), varlabels))
+  faclabels = sortDFG(setdiff(listNeighborhood(fg, varlabels, 1), varlabels))
 
   filter!(faclabels) do fl
     return issubset(getVariableOrder(fg, fl), varlabels)

IncrementalInference/src/parametric/services/ParametricUtils.jl

@@ -288,6 +288,7 @@ function buildGraphSolveManifold(vars::Vector{<:DFGVariable})
     N = vartypecount[vartype]
     G = getManifold(vartype)
     return NPowerManifold(G, N)
+    # return LieGroups.PowerLieGroup(G, N)
     # PowerManifold(G, NestedReplacingPowerRepresentation(), N)
     # PowerManifold(G, NestedPowerRepresentation(), N) #TODO investigate as it does not converge
   end

IncrementalInference/src/services/EvalFactor.jl

@@ -105,10 +105,10 @@ function addEntropyOnManifold!(
   end
 
   # preallocate 
-  T = number_eltype(points[1])
+  T = ManifoldsBase.number_eltype(points[1])
   Xc = zeros(T, manifold_dimension(M))
   #allocate to change SMatrix to MMatrix
-  X = allocate(get_vector(M, points[1], Xc, DefaultOrthogonalBasis()))
+  X = ManifoldsBase.allocate(get_vector(M, points[1], Xc, DefaultOrthogonalBasis()))
 
   for idx in 1:length(points)
     # build tangent coordinate random