AbstractCalcFactor and some manopt

Author: Affie

src/ExportAPI.jl

@@ -236,6 +236,7 @@ export CSMHistory,
 
   # Factor operational memory
   CommonConvWrapper,
+  AbstractCalcFactor,
   CalcFactor,
   getCliqVarInitOrderUp,
   getCliqNumAssocFactorsPerVar,

src/Factors/Circular.jl

@@ -21,7 +21,7 @@ CircularCircular(::UniformScaling) = CircularCircular(Normal())
 
 DFG.getManifold(::CircularCircular) = RealCircleGroup()
 
-function (cf::CalcFactor{<:CircularCircular})(X, p, q)
+function (cf::AbstractCalcFactor{<:CircularCircular})(X, p, q)
   #
   M = cf.manifold # getManifold(cf.factor)
   return distanceTangent2Point(M, X, p, q)

src/Factors/DefaultPrior.jl

@@ -14,7 +14,7 @@ getManifold(pr::Prior) = TranslationGroup(getDimension(pr.Z))
 # getSample(cf::CalcFactor{<:Prior}) = rand(cf.factor.Z)
 
 # basic default
-(s::CalcFactor{<:Prior})(z, x1) = z .- x1
+(s::AbstractCalcFactor{<:Prior})(z, x1) = z .- x1
 
 ## packed types are still developed by hand.  Future versions would likely use a @packable macro to write Protobuf safe versions of factors
 

src/Factors/EuclidDistance.jl

@@ -17,7 +17,7 @@ getManifold(::InstanceType{EuclidDistance}) = TranslationGroup(1)
 getDimension(::InstanceType{<:EuclidDistance}) = 1
 
 # new and simplified interface for both nonparametric and parametric
-(s::CalcFactor{<:EuclidDistance})(z, x1, x2) = z .- norm(x2 .- x1)
+(s::AbstractCalcFactor{<:EuclidDistance})(z, x1, x2) = z .- norm(x2 .- x1)
 
 function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{EuclidDistance})
   return Manifolds.TranslationGroup(1)

src/Factors/LinearRelative.jl

@@ -39,7 +39,7 @@ getManifold(::InstanceType{LinearRelative{N}}) where {N} = getManifold(Continuou
 getDimension(::InstanceType{LinearRelative{N}}) where {N} = N
 
 # new and simplified interface for both nonparametric and parametric
-function (s::CalcFactor{<:LinearRelative})(z, x1, x2)
+function (s::AbstractCalcFactor{<:LinearRelative})(z, x1, x2)
   # TODO convert to distance(distance(x2,x1),z) # or use dispatch on `-` -- what to do about `.-`
   # if s._sampleIdx < 5
   #   @info "LinearRelative" s._sampleIdx "$z" "$x1" "$x2" s.solvefor getLabel.(s.fullvariables)

src/Factors/MsgPrior.jl

@@ -32,7 +32,8 @@ end
 getManifold(mp::MsgPrior{<:ManifoldKernelDensity}) = mp.Z.manifold
 getManifold(mp::MsgPrior) = mp.M
 
-(cfo::CalcFactor{<:MsgPrior})(z, x1) = z .- x1
+#FIXME this will not work on manifolds
+(cfo::AbstractCalcFactor{<:MsgPrior})(z, x1) = z .- x1
 
 Base.@kwdef struct PackedMsgPrior <: AbstractPackedFactor
   Z::PackedSamplableBelief

src/entities/CalcFactor.jl

@@ -2,6 +2,8 @@
 abstract type AbstractMaxMixtureSolver end
 
 
+abstract type AbstractCalcFactor{T<:AbstractFactor} end
+
 """
 $TYPEDEF
 
@@ -32,7 +34,7 @@ struct CalcFactor{
   C, 
   VT <: Tuple, 
   M <: AbstractManifold
-}
+} <: AbstractCalcFactor{FT}
   """ the interface compliant user object functor containing the data and logic """
   factor::FT
   """ what is the sample (particle) id for which the residual is being calculated """
@@ -65,32 +67,57 @@ Related
 
 [`CalcFactor`](@ref)
 """
-struct CalcFactorMahalanobis{N, D, L, S <: Union{Nothing, AbstractMaxMixtureSolver}}
+struct CalcFactorMahalanobis{
+  FT,
+  N,
+  C,
+  MEAS<:AbstractArray,
+  D,
+  L,
+  S <: Union{Nothing, AbstractMaxMixtureSolver}
+} <: AbstractCalcFactor{FT}
   faclbl::Symbol
-  calcfactor!::CalcFactor
+  factor::FT
+  cache::C
   varOrder::Vector{Symbol}
-  meas::NTuple{N, <:AbstractArray}
+  meas::NTuple{N, MEAS}
   iΣ::NTuple{N, SMatrix{D, D, Float64, L}}
   specialAlg::S
 end
 
 
+# struct CalcFactorMahalanobis{N, D, L, S <: Union{Nothing, AbstractMaxMixtureSolver}} <: AbstractCalcFactor{FT}
+#   faclbl::Symbol
+#   calcfactor!::CalcFactor
+#   varOrder::Vector{Symbol}
+#   meas::NTuple{N, <:AbstractArray}
+#   iΣ::NTuple{N, SMatrix{D, D, Float64, L}}
+#   specialAlg::S
+# end
 
-
+#rename to CalcFactorResidual
 struct CalcFactorManopt{
+  FT <: AbstractFactor,
+  C,
   D,
   L,
-  FT <: AbstractFactor,
-  M <: AbstractManifold,
+  P,
   MEAS <: AbstractArray,
-}
+  N
+} <: AbstractCalcFactor{FT}
   faclbl::Symbol
-  calcfactor!::CalcFactor{FT, Nothing, Nothing, Tuple{}, M}
-  varOrder::Vector{Symbol}
-  varOrderIdxs::Vector{Int}
+  factor::FT
+  cache::C
+  varOrder::NTuple{N, Symbol}
+  varOrderIdxs::NTuple{N, Int}
+  points::P
   meas::MEAS
   iΣ::SMatrix{D, D, Float64, L}
   sqrt_iΣ::SMatrix{D, D, Float64, L}
 end
 
+_nvars(::CalcFactorManopt{FT, C, D, L, P, MEAS, N}) where {FT, C, D, L, P, MEAS, N} = N
+# _typeof_meas(::CalcFactorManopt{FT, C, D, L, MEAS, N}) where {FT, C, D, L, MEAS, N} = MEAS
+DFG.getDimension(::CalcFactorManopt{FT, C, D, L, P, MEAS, N}) where {FT, C, D, L, P, MEAS, N} = D
+
 

src/parametric/services/ParametricManoptDev.jl

@@ -11,7 +11,7 @@ using SparseArrays
 
 
 
-function CalcFactorManopt(fct::DFGFactor, varIntLabel)
+function CalcFactorManopt(fg, fct::DFGFactor, varIntLabel)
   fac_func = getFactorType(fct)
   varOrder = getVariableOrder(fct)
 
@@ -20,42 +20,28 @@ function CalcFactorManopt(fct::DFGFactor, varIntLabel)
   M = getManifold(getFactorType(fct))
 
   dims = manifold_dimension(M)
-  ϵ = getPointIdentity(M)
-
-  _meas, _iΣ = getMeasurementParametric(fct)
-  if fac_func isa ManifoldPrior
-    meas = _meas # already a point on M
-  elseif fac_func isa AbstractPrior
-    X = get_vector(M, ϵ, _meas, DefaultOrthonormalBasis())
-    meas = exp(M, ϵ, X) # convert to point on M
-  else
-    # its a relative factor so should be a tangent vector 
-    meas = convert(typeof(ϵ), get_vector(M, ϵ, _meas, DefaultOrthonormalBasis()))
-  end
 
-  # make sure its an SMatrix
-  iΣ = convert(SMatrix{dims, dims}, _iΣ)
+  meas, iΣ = getFactorMeasurementParametric(fct)
 
   sqrt_iΣ = convert(SMatrix{dims, dims}, sqrt(iΣ))
-  # cache = preambleCache(fg, getVariable.(fg, varOrder), getFactorType(fct))
+  cache = preambleCache(fg, getVariable.(fg, varOrder), getFactorType(fct))
 
-  calcf = CalcFactor(
+  return CalcFactorManopt(
+    fct.label,
     getFactorMechanics(fac_func),
-    0,
-    nothing,
-    true,
-    nothing,#cache,
-    (), #DFGVariable[],
-    0,
-    getManifold(fac_func),
+    cache,
+    varOrder,
+    varOrderIdxs,
+    meas,
+    iΣ,
+    sqrt_iΣ,
   )
-  return CalcFactorManopt(fct.label, calcf, varOrder, varOrderIdxs, meas, iΣ, sqrt_iΣ)
 end
 
 function (cfm::CalcFactorManopt)(p)
   meas = cfm.meas
   idx = cfm.varOrderIdxs
-  return cfm.sqrt_iΣ * cfm.calcfactor!(meas, p[idx]...)
+  return cfm.sqrt_iΣ * cfm(meas, p[idx]...)
 end
 
 # cost function f: M->ℝᵈ for Riemannian Levenberg-Marquardt 
@@ -83,7 +69,8 @@ struct JacF_RLM!{CF, T}
   res::Vector{Float64}
 end
 
-function JacF_RLM!(M, costF!; basis_domain::AbstractBasis = DefaultOrthonormalBasis())
+# function JacF_RLM!(M, costF!; basis_domain::AbstractBasis = DefaultOrthonormalBasis())
+function JacF_RLM!(M, costF!; basis_domain::AbstractBasis = DefaultOrthogonalBasis())
 
   p = costF!.points
 
@@ -108,7 +95,8 @@ function (jacF!::JacF_RLM!)(
   M::AbstractManifold,
   J,
   p;
-  basis_domain::AbstractBasis = DefaultOrthonormalBasis(),
+  # basis_domain::AbstractBasis = DefaultOrthonormalBasis(),
+  basis_domain::AbstractBasis = DefaultOrthogonalBasis(),
 )
 
   X0 = jacF!.X0
@@ -196,7 +184,7 @@ function solve_RLM(
   # varIntLabel_frontals = filter(p->first(p) in frontals, varIntLabel)
   # varIntLabel_separators = filter(p->first(p) in separators, varIntLabel)
 
-  calcfacs = map(f->CalcFactorManopt(f, varIntLabel), facs)
+  calcfacs = map(f->CalcFactorManopt(fg, f, varIntLabel), facs)
 
   # get the manifold and variable types
   frontal_vars = getVariable.(fg, frontals)
@@ -262,7 +250,7 @@ function solve_RLM_sparse(fg; kwargs...)
 
   varIntLabel = OrderedDict(zip(varlabels, collect(1:length(varlabels))))
 
-  calcfacs = CalcFactorManopt.(facs, Ref(varIntLabel))
+  calcfacs = CalcFactorManopt.(fg, facs, Ref(varIntLabel))
 
   # get the manifold and variable types
   vars = getVariable.(fg, varlabels)