Update to LieGroups (#1874)

Author: Affie

IncrementalInference/Project.toml

@@ -24,6 +24,7 @@ FunctionalStateMachine = "3e9e306e-7e3c-11e9-12d2-8f8f67a2f951"
 IncrementalInferenceTypes = "9808408f-4dbc-47e4-913c-6068b950e289"
 JSON3 = "0f8b85d8-7281-11e9-16c2-39a750bddbf1"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
+LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Logging = "56ddb016-857b-54e1-b83d-db4d58db5568"
 ManifoldDiff = "af67fdf4-a580-4b9f-bbec-742ef357defd"
@@ -86,11 +87,12 @@ Gadfly = "1"
 Interpolations = "0.15"
 JSON3 = "1"
 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/ext/HeatmapSampler.jl

@@ -80,7 +80,7 @@ end
 
 (lsg::LevelSetGridNormal)(w...; kw...) = lsg.densityFnc(w...; kw...)
 
-function sampleTangent(M::AbstractManifold, lsg::LevelSetGridNormal)
+function sampleTangent(M::AbstractLieGroup, lsg::LevelSetGridNormal)
   return sampleTangent(M, lsg.heatmap.densityFnc)
 end
 
@@ -203,7 +203,7 @@ function HeatmapGridDensity(
   bw = getBW(density_)[:, 1]
   @cast pts[i, j] := vec_preIS[j][i]
   bel = kde!(collect(pts), bw, weights)
-  density = ManifoldKernelDensity(TranslationGroup(Ndim(bel)), bel)
+  density = ManifoldKernelDensity(LieGroups.TranslationGroup(Ndim(bel)), bel)
 
   # return `<:SamplableBelief` object
   return HeatmapGridDensity(field_on_grid, domain, hint_callback, bw_factor, density)

IncrementalInference/ext/IncrInfrDiffEqFactorExt.jl

@@ -17,7 +17,7 @@ using DocStringExtensions
 
 export DERelative
 
-import Manifolds: allocate, compose, hat, Identity, vee, log
+using LieGroups: allocate, compose, hat, Identity, vee, log, LieAlgebra
 
 
 getManifold(de::DERelative{T}) where {T} = getManifold(de.domain)
@@ -233,7 +233,7 @@ function (cf::CalcFactor{<:DERelative})(
   # find the difference between measured and predicted.
   # assuming the ODE integrated from current X1 through to predicted X2 (ie `meas1[:,idx]`)
   res_ = compose(M, inv(M, X[solveforIdx]), meas1)
-  res = vee(M, Identity(M), log(M, Identity(M), res_))
+  res = vee(LieAlgebra(M), log(M, res_))
 
   return res
 end
@@ -301,7 +301,7 @@ function IncrementalInference.sampleFactor(cf::CalcFactor{<:DERelative}, N::Int
     prob = oder.backwardProblem
     M_ = getManifold(getVariableType(cf.fullvariables[1]))
     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
-      convert(Tuple, M_),
+      AMP._manifoldtuple(M_),
     )
     # getBelief(cf.fullvariables[2]) |> getPoints
     cf._legacyParams[2], M_
@@ -311,7 +311,7 @@ function IncrementalInference.sampleFactor(cf::CalcFactor{<:DERelative}, N::Int
     M_ = getManifold(getVariableType(cf.fullvariables[2]))
     # buffer manifold operations for use during factor evaluation
     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
-      convert(Tuple, M_),
+      AMP._manifoldtuple(M_),
     )
     # getBelief(cf.fullvariables[1]) |> getPoints
     cf._legacyParams[1], M_

IncrementalInference/ext/IncrInfrFluxFactorsExt.jl

@@ -9,6 +9,7 @@ using DataStructures: OrderedDict
 using LinearAlgebra
 using Base64
 using Manifolds
+using LieGroups
 using DocStringExtensions
 using BSON
 
@@ -51,8 +52,10 @@ function Random.rand(nfb::FluxModelsDistribution, N::Integer = 1)
 end
 
 sampleTangent(M::AbstractManifold, fmd::FluxModelsDistribution, p = 0) = rand(fmd, 1)[1]
+sampleTangent(M::AbstractLieGroup, fmd::FluxModelsDistribution, p = 0) = rand(fmd, 1)[1]
+
 samplePoint(M::AbstractManifold, fmd::FluxModelsDistribution, p = 0) = rand(fmd, 1)[1]
-function samplePoint(M::AbstractDecoratorManifold, fmd::FluxModelsDistribution, p = 0)
+function samplePoint(M::AbstractLieGroup, fmd::FluxModelsDistribution, p = 0)
   return rand(fmd, 1)[1]
 end
 

IncrementalInference/ext/IncrInfrInterpolationsExt.jl

@@ -7,6 +7,8 @@ using Statistics
 using DocStringExtensions
 using TensorCast
 using Manifolds
+using LieGroups
+using LieGroups: TranslationGroup
 using ApproxManifoldProducts
 import ApproxManifoldProducts: sample
 const AMP = ApproxManifoldProducts

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,12 +4,13 @@
 
 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})
   # FIXME workaround for issue with manifolds CircularGroup, 
-  return [rand(cf.factor.Z)]
+  # return [rand(cf.factor.Z)]
+  return sampleTangent(getManifold(cf), cf.factor.Z, getPointIdentity(Circular))
 end
 
 function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{CircularCircular})
@@ -28,13 +29,14 @@ function getSample(cf::CalcFactor{<:PriorCircular})
   # FIXME workaround for issue #TBD with manifolds CircularGroup, 
   # JuliaManifolds/Manifolds.jl#415
   # no method similar(::Float64, ::Type{Float64})
-  return samplePoint(cf.manifold, cf.factor.Z, [0.0])
+  # return samplePoint(cf.manifold, cf.factor.Z, [0.0])
+  return samplePoint(cf.manifold, cf.factor.Z, getPointIdentity(Circular))
   # return [Manifolds.sym_rem(rand(cf.factor.Z))]
 end
 
 function (cf::CalcFactor{<:PriorCircular})(m, p)
   M = getManifold(cf)
-  Xc = vee(M, p, log(M, p, m))
+  Xc = vee(LieAlgebra(M), log(M, p, m))
   return Xc
 end
 

IncrementalInference/src/Factors/EuclidDistance.jl

@@ -20,7 +20,7 @@ getDimension(::InstanceType{<:EuclidDistance}) = 1
 (s::CalcFactor{<:EuclidDistance})(z, x1, x2) = z .- norm(x2 .- x1)
 
 function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{EuclidDistance})
-  return Manifolds.TranslationGroup(1)
+  return LieGroups.TranslationGroup(1)
 end
 
 """

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,29 @@ 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
+# """
+#     $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̂)
+# ::MeasurementOnTangent
+function measurement_residual(G::AbstractLieGroup, X, p, q)
+  X̂ = log(G, p, q)
+  return vee(LieAlgebra(G), X - X̂) # TODO check sign with gradients, does not matter for cost so can't double check.
 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 prior_residual(G::AbstractLieGroup, m, p)
+  #TODO should it be TₘM or TₚM?
+  # Is the covariance that of the point m? If so, I would think it should be TₘM, but that doesn't seem to work.
+  X = log(G, p, m) # X ∈ TₚM, # this one gives the correct hex.
+  # X = log(G, m, p) # X ∈ TₘM, 
+  return vee(LieAlgebra(G), X)
 end
 
 """
@@ -69,10 +67,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 +91,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 +140,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 +161,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::AbstractLieGroup, p, Z)
+  return ManifoldPrior(M, p, Z, DefaultLieAlgebraOrthogonalBasis(), MB.ExponentialRetraction())
 end
 
 DFG.getManifold(f::ManifoldPrior) = f.M
@@ -186,31 +179,38 @@ DFG.getManifold(f::ManifoldPrior) = f.M
 function getSample(cf::CalcFactor{<:ManifoldPrior})
   Z = cf.factor.Z
   p = cf.factor.p
-  M = cf.manifold # .factor.M
+  M = cf.factor.M
   basis = cf.factor.basis
   retract_method = cf.factor.retract_method
   point = samplePoint(M, Z, p, basis, retract_method)
 
   return point
 end
 
+function getSample(cf::CalcFactor{<:ManifoldPrior{<:AbstractLieGroup}})
+  Z = cf.factor.Z
+  p = cf.factor.p
+  M = cf.factor.M
+  point = samplePoint(M, Z, p)
+
+  return point
+end
+
 function getFactorMeasurementParametric(fac::ManifoldPrior)
   M = getManifold(fac)
   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
 # dim = manifold_dimension(M)
 # Xc = [SVector{dim}(rand(Z)) for _ in 1:N]
 
-function (cf::CalcFactor{<:ManifoldPrior})(m, p)
+function (cf::CalcFactor{<:ManifoldPrior{<:AbstractLieGroup}})(m, p)
   M = cf.factor.M
-  # return log(M, p, m)
-  return vee(M, p, log(M, p, m))
-  # return distancePrior(M, m, p)
+  return prior_residual(M, m, p)
 end
 
 # dist²_Σ = ⟨X, Σ⁻¹*X'⟩

IncrementalInference/src/Factors/GenericMarginal.jl

@@ -11,7 +11,7 @@ mutable struct GenericMarginal <: RelativeObservation # AbstractRelativeRoots
   GenericMarginal(a, b, c) = new(a, b, c)
 end
 
-getManifold(::GenericMarginal) = TranslationGroup(1)
+getManifold(::GenericMarginal) = LieGroups.TranslationGroup(1)
 
 getSample(::CalcFactor{<:GenericMarginal}) = [0]