test SE2Mani works as a mix beteween SO(2) x Tr(2) and SE(2) and some vee/hat

Author: Affie

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
 
 
 getManifold(de::DERelative{T}) where {T} = getManifold(de.domain)

IncrementalInference/src/Factors/Circular.jl

@@ -34,7 +34,7 @@ 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/GenericFunctions.jl

@@ -36,9 +36,17 @@ DFG.getDimension(Z::BallTreeDensity) = Ndim(Z)
 # end
 
 # ::MeasurementOnTangent
-function measurement_residual(G::LieGroup, X, p, q)
+function measurement_residual(G::AbstractLieGroup, X, p, q)
   X̂ = log(G, p, q)
-  return vee(LieAlgebra(G), p, X - X̂) # TODO check sign
+  return vee(LieAlgebra(G), X - X̂) # TODO check sign with gradients, does not matter for cost so can't double check.
+end
+
+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
 
 """
@@ -153,8 +161,8 @@ struct ManifoldPrior{M <: AbstractManifold, T <: SamplableBelief, P, B <: Abstra
   retract_method::AbstractRetractionMethod
 end
 
-function ManifoldPrior(M::LieGroup, p, Z)
-  return ManifoldPrior(M, p, Z, MB.VeeOrthogonalBasis(), MB.ExponentialRetraction())
+function ManifoldPrior(M::AbstractLieGroup, p, Z)
+  return ManifoldPrior(M, p, Z, DefaultLieAlgebraOrthogonalBasis(), MB.ExponentialRetraction())
 end
 
 DFG.getManifold(f::ManifoldPrior) = f.M
@@ -171,14 +179,23 @@ 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)
@@ -191,11 +208,9 @@ end
 # 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/IncrementalInference.jl

@@ -14,12 +14,12 @@ using Reexport
 @reexport using LinearAlgebra
 
 import Manifolds
-using Manifolds: ProductGroup, AbstractDecoratorManifold, get_vector, get_coordinates, ProductManifold, PowerManifold, GeodesicInterpolation, DefaultOrthogonalBasis, get_vector!
+using Manifolds: ProductGroup, AbstractDecoratorManifold, get_vector, 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: AbstractLieGroup, LieGroup, LieAlgebra, ProductLieGroup, hat, vee, compose, AbstractProductGroupOperation, AdditionGroupOperation, SpecialEuclideanGroup, SpecialOrthogonalGroup, DefaultLieAlgebraOrthogonalBasis
 using LieGroups: TranslationGroup
 # using LieGroups: ProductGroupOperation, SemiDirectProductGroupOperation
 

IncrementalInference/src/manifolds/services/ManifoldSampling.jl

@@ -43,11 +43,11 @@ function sampleTangent(
   # 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 = getPointIdentity(M))
+function sampleTangent(M::AbstractLieGroup, 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))
+function sampleTangent(M::AbstractLieGroup, 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)
@@ -84,7 +84,7 @@ function samplePoint(
 end
 
 function samplePoint(
-  M::LieGroup,
+  M::AbstractLieGroup,
   sbelief,
   p = getPointIdentity(M),
   retraction_method::AbstractRetractionMethod = ExponentialRetraction(),

IncrementalInference/src/manifolds/services/ManifoldsExtentions.jl

@@ -104,7 +104,7 @@ function DFG.getPointIdentity(G::ProductGroup, ::Type{T} = Float64) where {T <:
 end
 
 # fallback 
-function DFG.getPointIdentity(G::LieGroup, ::Type{T} = Float64) where {T <: Real}
+function DFG.getPointIdentity(G::AbstractLieGroup, ::Type{T} = Float64) where {T <: Real}
   return error("getPointIdentity not implemented on $G")
 end
 
@@ -117,14 +117,22 @@ end
 
 #TODO test
 function DFG.getPointIdentity(
-  PrG::LieGroup{𝔽, Op, M},
+  PrG::AbstractLieGroup{𝔽, 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)
+  ε = map(G -> getPointIdentity(G, T), map(LieGroup, PrM.manifolds, PrG.op.operations))
   return ArrayPartition(ε)
 end
 
+function DFG.getPointIdentity(
+  VG::LieGroups.ValidationLieGroup,
+  ::Type{T} = Float64,
+) where {T <: Real}
+  G = VG.lie_group
+  return LieGroups.ValidationMPoint(getPointIdentity(G, T))
+end
+
 function DFG.getPointIdentity(
   @nospecialize(M::PowerManifold),
   ::Type{T} = Float64,
@@ -167,6 +175,6 @@ function DFG.getPointIdentity(
   return zeros(SVector{N, T})
 end
 
-function DFG.getPointIdentity(G::LieGroup{ℝ,AdditionGroupOperation,<:Circle{ℝ}}, ::Type{T} = Float64) where {T <: Real}
+function DFG.getPointIdentity(G::AbstractLieGroup{ℝ,AdditionGroupOperation,<:Circle{ℝ}}, ::Type{T} = Float64) where {T <: Real}
   return [zero(T)] #FIXME we cannot support scalars yet
 end

IncrementalInference/src/parametric/services/ParametricManopt.jl

@@ -162,7 +162,7 @@ end
 # function JacF_RLM!(M, costF!; basis_domain::AbstractBasis = DefaultOrthonormalBasis())
 function JacF_RLM!(M, costF!, p, fg=nothing;
   all_points=p,
-  basis_domain::AbstractBasis = DefaultOrthogonalBasis(),
+  basis_domain::AbstractBasis = LieGroups.DefaultLieAlgebraOrthogonalBasis(),
   is_sparse=!isnothing(fg)
 )
 
@@ -171,6 +171,7 @@ function JacF_RLM!(M, costF!, p, fg=nothing;
   X0 = zeros(manifold_dimension(M))
 
   X = get_vector(M, p, X0, basis_domain)
+  # X = vee(LieAlgebra(M), X0)
 
   q = exp(M, p, X)
 

IncrementalInference/src/parametric/services/ParametricUtils.jl

@@ -104,7 +104,8 @@ function getFactorMeasurementParametric(fac::AbstractPrior)
   ϵ = getPointIdentity(M)
   dims = manifold_dimension(M)
   Xc, iΣ = getMeasurementParametric(fac)
-  X = get_vector(M, ϵ, Xc, DefaultOrthogonalBasis())
+  # X = get_vector(M, ϵ, Xc, DefaultOrthogonalBasis())
+  X = hat(LieAlgebra(M), Xc, typeof(ϵ))
   meas = convert(typeof(ϵ), exp(M, ϵ, X))
   iΣ = convert(SMatrix{dims, dims}, iΣ)
   meas, iΣ
@@ -115,7 +116,12 @@ function getFactorMeasurementParametric(fac::AbstractRelative)
   ϵ = getPointIdentity(M)
   dims = manifold_dimension(M)
   Xc, iΣ = getMeasurementParametric(fac)
-  measX = convert(typeof(ϵ), get_vector(M, ϵ, Xc, DefaultOrthogonalBasis()))
+  # measX = convert(typeof(ϵ), get_vector(M, ϵ, Xc, DefaultOrthogonalBasis()))
+  if M isa LieGroups.ValidationLieGroup
+    measX = LieGroups.unwrap_validation(hat(LieAlgebra(M), Xc, typeof(ϵ)))
+  else
+    measX = convert(typeof(ϵ), hat(LieAlgebra(M), Xc, typeof(ϵ)))
+  end
   iΣ = convert(SMatrix{dims, dims}, iΣ)
   measX, iΣ
 end
@@ -287,12 +293,18 @@ function buildGraphSolveManifold(vars::Vector{<:VariableCompute})
   PMs = map(vartypes) do vartype
     N = vartypecount[vartype]
     G = getManifold(vartype)
+    if G isa LieGroups.ValidationLieGroup
+      #strip away ValidationLieGroup
+      G = G.lie_group
+    end
     return NPowerManifold(G, N)
     # return LieGroups.PowerLieGroup(G, N)
+    # return Manifolds.PowerManifold(G, N)
     # PowerManifold(G, NestedReplacingPowerRepresentation(), N)
     # PowerManifold(G, NestedPowerRepresentation(), N) #TODO investigate as it does not converge
   end
   M = ProductManifold(PMs...)
+  # M = ProductLieGroup(PMs...)
   return M, vartypes, vartypeslist
 end
 
@@ -307,7 +319,8 @@ function GraphSolveBuffers(@nospecialize(M), ::Type{T}) where {T}
   ϵ = getPointIdentity(M, T)
   p = deepcopy(ϵ)# allocate_result(M, getPointIdentity)
   X = deepcopy(ϵ) #allcoate(p)
-  Xc = get_coordinates(M, ϵ, X, DefaultOrthogonalBasis())
+  # Xc = Manifolds.get_coordinates(M, ϵ, X, DefaultOrthogonalBasis())
+  Xc = vee(LieGroup(M), X)
   return GraphSolveBuffers(ϵ, p, X, Xc)
 end
 
@@ -569,7 +582,8 @@ function solveGraphParametricOptim(
   # log!(M, X, Identity(ProductOperation), p)
   # calculate initial coordinates vector for Optim
   log!(M, X, ϵ, p)
-  get_coordinates!(M, Xc, ϵ, X, DefaultOrthogonalBasis())
+  # get_coordinates!(M, Xc, ϵ, X, DefaultOrthogonalBasis())
+  vee!(LieGroup(M), Xc, X)
 
   initValues = Xc
   #FIXME, for some reason we get NANs and adding a small random value works

IncrementalInference/src/services/NumericalCalculations.jl

@@ -103,7 +103,8 @@ end
 function (hypoCalcFactor::CalcFactorNormSq)(M::AbstractManifold, Xc::AbstractVector)
   # hypoCalcFactor.manifold is the factor's manifold, not the variable's manifold that is needed here
   ϵ = getPointIdentity(M)
-  X = get_vector(M, ϵ, SVector(Xc), DefaultOrthogonalBasis())
+  # X = get_vector(M, ϵ, SVector(Xc), DefaultOrthogonalBasis())
+  X = hat(LieAlgebra(M), SVector(Xc), typeof(ϵ))
   p = exp(M, ϵ, X)
   return hypoCalcFactor(CalcConv, p)
 end
@@ -131,7 +132,7 @@ function _solveLambdaNumeric(
   ϵ = getPointIdentity(variableType)
 
   X0c = zero(MVector{getDimension(M),Float64})
-  X0c .= vee(M, u0, log(M, ϵ, u0))
+  X0c .= vee(LieAlgebra(M), log(M, ϵ, u0))
 
   alg = islen1 ? Optim.BFGS() : Optim.NelderMead()
 
@@ -171,7 +172,8 @@ end
 # for deconv with the measurement a tangent vector, can dispatch for other measurement types.
 function (hypoCalcFactor::CalcFactorNormSq)(::Type{CalcDeconv}, M::AbstractManifold, Xc::AbstractVector)
   ϵ = getPointIdentity(M)
-  X = get_vector(M, ϵ, Xc, DefaultOrthogonalBasis())
+  # X = get_vector(M, ϵ, Xc, DefaultOrthogonalBasis())
+  X = hat(LieAlgebra(M), Xc, typeof(ϵ))
   return hypoCalcFactor(CalcDeconv, X)
 end
 
@@ -188,7 +190,7 @@ function _solveLambdaNumericMeas(
   M = getManifold(fcttype)
   ϵ = getPointIdentity(M)
   X0c = zeros(manifold_dimension(M))
-  X0c .= vee(M, ϵ, X0)
+  X0c .= vee(LieAlgebra(M), X0)
 
   alg = islen1 ? Optim.BFGS() : Optim.NelderMead()
 

IncrementalInference/src/services/SubGraphFunctions.jl

@@ -141,9 +141,8 @@ function transferUpdateSubGraph!(
     @info "transferUpdateSubGraph! -- syms=$syms"
   end
 
-  # transfer specific fields into dest from src
-  @time for var in (x -> getVariable(src, x)).(syms)
-    # NOTE compared copytoState! vs surgical updateVariableSolverData!
+  for var in (x -> getVariable(src, x)).(syms)
+    # NOTE compared copytoState! vs deprecated surgical updateVariableSolverData!
     # updateVariableSolverData! 0.000626 seconds (1.11 k allocations: 114.289 KiB)
     # copytoState! 0.000099 seconds (315 allocations: 27.758 KiB)
     DFG.copytoState!(