Swap parametric optim for manopt and better convolution performance

benchmark/runbenchmarks.jl

@@ -35,7 +35,7 @@ fg.solverParams.graphinit=false;
 addVariable!(fg, :x0, Pose2);
 addFactor!(fg, [:x0], PriorPose2(MvNormal([0.0,0,0], diagm([0.1,0.1,0.01].^2))));
 
-r = @timed IIF.solveGraphParametric!(fg; init=false);
+r = @timed IIF.solveGraphParametric!(fg; init=false, is_sparse=false);
 
 timed = [r];
 
@@ -44,7 +44,7 @@ for i = 1:14
     to = Symbol("x",i)
     addVariable!(fg, to, Pose2)
     addFactor!(fg, [fr,to], Pose2Pose2(MvNormal([10.0,0,pi/3], diagm([0.5,0.5,0.05].^2))))
-    r = @timed IIF.solveGraphParametric!(fg; init=false);
+    r = @timed IIF.solveGraphParametric!(fg; init=false, is_sparse=false);
     push!(timed, r)
 end
 
@@ -53,7 +53,7 @@ addVariable!(fg, :l1, RoME.Point2, tags=[:LANDMARK;]);
 addFactor!(fg, [:x0; :l1], Pose2Point2BearingRange(Normal(0.0,0.1), Normal(20.0, 1.0)));
 addFactor!(fg, [:x6; :l1], Pose2Point2BearingRange(Normal(0.0,0.1), Normal(20.0, 1.0)));
 
-r = @timed IIF.solveGraphParametric!(fg; init=false);
+r = @timed IIF.solveGraphParametric!(fg; init=false, is_sparse=false);
 push!(timed, r);
 
 getproperty.(timed, :time)

src/Factors/Circular.jl

@@ -23,7 +23,7 @@
 
 function (cf::CalcFactor{<:CircularCircular})(X, p, q)
   #
-  M = cf.manifold # getManifold(cf.factor)
+  M = getManifold(cf)
   return distanceTangent2Point(M, X, p, q)
 end
 
@@ -68,7 +68,7 @@
 end
 
 function (cf::CalcFactor{<:PriorCircular})(m, p)
-  M = cf.manifold # getManifold(cf.factor)
+  M = getManifold(cf)
   Xc = vee(M, p, log(M, p, m))
   return Xc
 end

src/Factors/GenericFunctions.jl

@@ -243,7 +243,7 @@
 
 function getSample(cf::CalcFactor{<:ManifoldPriorPartial})
   Z = cf.factor.Z
-  M = cf.manifold # getManifold(cf.factor)
+  M = getManifold(cf)
   partial = collect(cf.factor.partial)
 
   return (samplePointPartial(M, Z, partial),)

src/manifolds/services/ManifoldSampling.jl

@@ -119,7 +119,7 @@ See also: [`getMeasurementParametric`](@ref)
 function getSample end
 
 function getSample(cf::CalcFactor{<:AbstractPrior})
-  M = cf.manifold # getManifold(cf.factor)
+  M = getManifold(cf)
   if hasfield(typeof(cf.factor), :Z)
     X = samplePoint(M, cf.factor.Z)
   else
@@ -132,7 +132,7 @@ function getSample(cf::CalcFactor{<:AbstractPrior})
 end
 
 function getSample(cf::CalcFactor{<:AbstractRelative})
-  M = cf.manifold # getManifold(cf.factor)
+  M =getManifold(cf)
   if hasfield(typeof(cf.factor), :Z)
     X = sampleTangent(M, cf.factor.Z)
   else

src/parametric/services/ParametricCSMFunctions.jl

@@ -26,7 +26,7 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer)
   # store the cliqSubFg for later debugging
   _dbgCSMSaveSubFG(csmc, "fg_beforeupsolve")
 
-  vardict, result, varIds, Σ = solveGraphParametric(csmc.cliqSubFg)
+  vardict, result, varIds, Σ = solveGraphParametricOptim(csmc.cliqSubFg)
 
   logCSM(csmc, "$(csmc.cliq.id) vars $(keys(varIds))")
   # @info "$(csmc.cliq.id) Σ $(Σ)"

src/parametric/services/ParametricManopt.jl

@@ -297,8 +297,13 @@
     H = FiniteDiff.finite_difference_hessian(costf, X0)
 
     # inv(H)
-    Σ = Matrix(H) \ Matrix{eltype(H)}(I, size(H)...)
-    # sqrt.(diag(Σ))
+    Σ = try 
+        Matrix(H) \ Matrix{eltype(H)}(I, size(H)...)
+      catch ex #TODO only catch correct exception and try with pinv as fallback in certain cases.
+        @warn "Hessian inverse failed" ex
+        # Σ = pinv(H)
+        nothing
+      end
     return Σ
 end
 
@@ -490,27 +495,28 @@
   # new2    0.010764 seconds (34.61 k allocations: 3.111 MiB)
   # dense  J 0.022079 seconds (283.54 k allocations: 18.146 MiB)
 
-function autoinitParametricManopt!(
+function autoinitParametric!(
   fg,
   varorderIds = getInitOrderParametric(fg);
   reinit = false,
   kwargs...
 )
   @showprogress for vIdx in varorderIds
-    autoinitParametricManopt!(fg, vIdx; reinit, kwargs...)
+    autoinitParametric!(fg, vIdx; reinit, kwargs...)
   end
   return nothing
 end
 
-function autoinitParametricManopt!(dfg::AbstractDFG, initme::Symbol; kwargs...)
-  return autoinitParametricManopt!(dfg, getVariable(dfg, initme); kwargs...)
+function autoinitParametric!(dfg::AbstractDFG, initme::Symbol; kwargs...)
+  return autoinitParametric!(dfg, getVariable(dfg, initme); kwargs...)
 end
 
-function autoinitParametricManopt!(
+function autoinitParametric!(
   dfg::AbstractDFG,
   xi::DFGVariable;
   solveKey = :parametric,
   reinit::Bool = false,
+  perturb_point::Bool=false,
   kwargs...,
 )
   #
@@ -528,12 +534,27 @@
       return isInitialized(dfg, vl, solveKey)
     end
 
+    vnd::VariableNodeData = getSolverData(xi, solveKey)
+
+    if perturb_point
+      _M = getManifold(xi)
+      p = vnd.val[1]
+      vnd.val[1] = exp(
+        _M,
+        p, 
+        get_vector(
+          _M,
+          p,
+          randn(manifold_dimension(_M))*10^-6,
+          DefaultOrthogonalBasis()
+        )
+      )
+    end
     M, vartypeslist, lm_r, Σ = solve_RLM_conditional(dfg, [initme], initfrom; kwargs...)
-
+    
     val = lm_r[1]
-    vnd::VariableNodeData = getSolverData(xi, solveKey)
     vnd.val[1] = val
-    
+
     !isnothing(Σ) && vnd.bw .= Σ
 
     # updateSolverDataParametric!(vnd, val, Σ)
@@ -555,6 +576,7 @@
 
 
 ##
+solveGraphParametric(args...; kwargs...) = solve_RLM(args...; kwargs...)
 
 function DFG.solveGraphParametric!(
   fg::AbstractDFG,
@@ -578,7 +600,7 @@
 
   updateParametricSolution!(fg, M, v, r, Σ)
 
-  return v,r, Σ 
+  return M, v, r, Σ 
 end
 
 

src/parametric/services/ParametricUtils.jl

@@ -530,7 +530,7 @@
   return subsigmas
 end
 
-function solveGraphParametric(
+function solveGraphParametricOptim(
   fg::AbstractDFG;
   verbose::Bool = false,
   computeCovariance::Bool = true,
@@ -828,8 +828,7 @@
     $SIGNATURES
 Add parametric solver to fg, batch solve using [`solveGraphParametric`](@ref) and update fg.
 """
-function DFG.solveGraphParametric!(
-  ::Val{:Optim},
+function solveGraphParametricOptim!(
   fg::AbstractDFG; 
   init::Bool = true, 
   solveKey::Symbol = :parametric, # FIXME, moot since only :parametric used for parametric solves
@@ -845,7 +844,7 @@
     initParametricFrom!(fg, initSolveKey; parkey=solveKey)
   end
 
-  vardict, result, varIds, Σ = solveGraphParametric(fg; verbose, kwargs...)
+  vardict, result, varIds, Σ = solveGraphParametricOptim(fg; verbose, kwargs...)
 
   updateParametricSolution!(fg, vardict)
 
@@ -970,7 +969,7 @@
   return order
 end
 
-function autoinitParametric!(
+function autoinitParametricOptim!(
   fg,
   varorderIds = getInitOrderParametric(fg);
   reinit = false,
@@ -979,16 +978,16 @@
   kwargs...
 )
   @showprogress for vIdx in varorderIds
-    autoinitParametric!(fg, vIdx; reinit, algorithm, algorithmkwargs, kwargs...)
+    autoinitParametricOptim!(fg, vIdx; reinit, algorithm, algorithmkwargs, kwargs...)
   end
   return nothing
 end
 
-function autoinitParametric!(dfg::AbstractDFG, initme::Symbol; kwargs...)
-  return autoinitParametric!(dfg, getVariable(dfg, initme); kwargs...)
+function autoinitParametricOptim!(dfg::AbstractDFG, initme::Symbol; kwargs...)
+  return autoinitParametricOptim!(dfg, getVariable(dfg, initme); kwargs...)
 end
 
-function autoinitParametric!(
+function autoinitParametricOptim!(
   dfg::AbstractDFG,
   xi::DFGVariable;
   solveKey = :parametric,

src/services/CalcFactor.jl

@@ -192,7 +192,7 @@ end
 
 # the same as legacy, getManifold(ccwl.usrfnc!)
 getManifold(ccwl::CommonConvWrapper) = ccwl.manifold
-getManifold(cf::CalcFactor) = cf.manifold
+getManifold(cf::CalcFactor) = getManifold(cf.factor)
 
 function _resizePointsVector!(
   vecP::AbstractVector{P},

src/services/NumericalCalculations.jl

@@ -133,6 +133,14 @@ function _solveLambdaNumeric_original(
   return exp(M, ϵ, hat(M, ϵ, r.minimizer))
 end
 
+function cost_optim(M, objResX, Xc)
+  ϵ = getPointIdentity(M)
+  X = get_vector(M, ϵ, Xc, DefaultOrthogonalBasis())
+  p = exp(M, ϵ, X)  
+  residual = objResX(p)
+  return sum(x->x^2, residual) #TODO maybe move this to CalcFactorNormSq
+end
+
 # 1.355700 seconds (11.78 M allocations: 557.677 MiB, 6.96% gc time)
 function _solveCCWNumeric_test_SA(
   fcttype::Union{F, <:Mixture{N_, F, S, T}},
@@ -153,18 +161,9 @@ function _solveCCWNumeric_test_SA(
   X0c = zero(MVector{getDimension(M),Float64})
   X0c .= vee(M, u0, log(M, ϵ, u0))
 
-  #TODO check performance
-  function cost(Xc)
-    X = hat(M, ϵ, Xc)
-    p = exp(M, ϵ, X)  
-    residual = objResX(p)
-    # return sum(residual .^ 2)
-    return sum(abs2, residual) #TODO maybe move this to CalcFactorNormSq
-  end
-
   alg = islen1 ? Optim.BFGS() : Optim.NelderMead()
 
-  r = Optim.optimize(cost, X0c, alg)
+  r = Optim.optimize(x->cost_optim(M, objResX, x), X0c, alg)
   if !Optim.converged(r)
     # TODO find good way for a solve to store diagnostics about number of failed converges etc.
     @warn "Optim did not converge (maxlog=10):" r maxlog=10

src/services/VariableStatistics.jl

@@ -20,12 +20,15 @@ end
 
 #TODO check performance and FIXME on makemutalbe might not be needed any more
 function calcStdBasicSpread(vartype::InferenceVariable, ptsArr::AbstractVector) # {P}) where {P}
-  _makemutable(s) = s
-  _makemutable(s::StaticArray{Tuple{S},T,N}) where {S,T,N} = MArray{Tuple{S},T,N,S}(s)
-  _makemutable(s::SMatrix{N,N,T,D}) where {N,T,D} = MMatrix{N,N,T,D}(s)
+  # _makemutable(s) = s
+  # _makemutable(s::StaticArray{Tuple{S},T,N}) where {S,T,N} = MArray{Tuple{S},T,N,S}(s)
+  # _makemutable(s::SMatrix{N,N,T,D}) where {N,T,D} = MMatrix{N,N,T,D}(s)
 
   # FIXME, silly conversion since Manifolds.std internally replicates eltype ptsArr which doesn't work on StaticArrays
-  σ = std(vartype, _makemutable.(ptsArr))
+  # σ = std(vartype, _makemutable.(ptsArr))
+
+  μ = mean(vartype, ptsArr, GeodesicInterpolation())
+  σ = std(vartype, ptsArr, μ)
 
   #if no std yet, set to 1
   msst = 1e-10 < σ ? σ : 1.0