From c49e4b0faffd8a3eed55fcfe4fa265823b2c0724 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche <6612981+Affie@users.noreply.github.com> Date: Fri, 13 Oct 2023 16:17:53 +0200 Subject: [PATCH] Swop parametric optim for manopt and better convolution performance (#1784) * swop parametric optim for manopt * improve non-parametric performance * Standardise to use getManifold * covariance calc try catch on hessian inverse --- benchmark/runbenchmarks.jl | 6 +-- src/Factors/Circular.jl | 4 +- src/Factors/GenericFunctions.jl | 2 +- src/manifolds/services/ManifoldSampling.jl | 4 +- .../services/ParametricCSMFunctions.jl | 2 +- src/parametric/services/ParametricManopt.jl | 44 ++++++++++++---- src/parametric/services/ParametricUtils.jl | 17 +++---- src/services/CalcFactor.jl | 2 +- src/services/NumericalCalculations.jl | 19 ++++--- src/services/VariableStatistics.jl | 11 ++-- test/testBasicParametric.jl | 51 ++++++++++--------- 11 files changed, 94 insertions(+), 68 deletions(-) diff --git a/benchmark/runbenchmarks.jl b/benchmark/runbenchmarks.jl index 035af1ba..ce033483 100644 --- a/benchmark/runbenchmarks.jl +++ b/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) diff --git a/src/Factors/Circular.jl b/src/Factors/Circular.jl index 436d9984..4de0323d 100644 --- a/src/Factors/Circular.jl +++ b/src/Factors/Circular.jl @@ -23,7 +23,7 @@ DFG.getManifold(::CircularCircular) = RealCircleGroup() 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 @@ function getSample(cf::CalcFactor{<:PriorCircular}) 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 diff --git a/src/Factors/GenericFunctions.jl b/src/Factors/GenericFunctions.jl index 4592dae9..ef8b5cbb 100644 --- a/src/Factors/GenericFunctions.jl +++ b/src/Factors/GenericFunctions.jl @@ -243,7 +243,7 @@ DFG.getManifold(f::ManifoldPriorPartial) = f.M 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),) diff --git a/src/manifolds/services/ManifoldSampling.jl b/src/manifolds/services/ManifoldSampling.jl index 14c8932d..c18de24e 100644 --- a/src/manifolds/services/ManifoldSampling.jl +++ b/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 diff --git a/src/parametric/services/ParametricCSMFunctions.jl b/src/parametric/services/ParametricCSMFunctions.jl index 52f10f59..6a6bac75 100644 --- a/src/parametric/services/ParametricCSMFunctions.jl +++ b/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) Σ $(Σ)" diff --git a/src/parametric/services/ParametricManopt.jl b/src/parametric/services/ParametricManopt.jl index 170fda1c..1e4c8d2d 100644 --- a/src/parametric/services/ParametricManopt.jl +++ b/src/parametric/services/ParametricManopt.jl @@ -297,8 +297,13 @@ function covarianceFiniteDiff(M, jacF!::JacF_RLM!, p0) 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 @@ end # 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 @@ function autoinitParametricManopt!( 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 @@ end ## +solveGraphParametric(args...; kwargs...) = solve_RLM(args...; kwargs...) function DFG.solveGraphParametric!( fg::AbstractDFG, @@ -578,7 +600,7 @@ function DFG.solveGraphParametric!( updateParametricSolution!(fg, M, v, r, Σ) - return v,r, Σ + return M, v, r, Σ end diff --git a/src/parametric/services/ParametricUtils.jl b/src/parametric/services/ParametricUtils.jl index 4159968d..9a9d43e4 100644 --- a/src/parametric/services/ParametricUtils.jl +++ b/src/parametric/services/ParametricUtils.jl @@ -530,7 +530,7 @@ function _getComponentsCovar(@nospecialize(PM::NPowerManifold), Σ::AbstractMatr return subsigmas end -function solveGraphParametric( +function solveGraphParametricOptim( fg::AbstractDFG; verbose::Bool = false, computeCovariance::Bool = true, @@ -828,8 +828,7 @@ end $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 @@ function DFG.solveGraphParametric!( 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 @@ function getInitOrderParametric(fg; startIdx::Symbol = lsfPriors(fg)[1]) return order end -function autoinitParametric!( +function autoinitParametricOptim!( fg, varorderIds = getInitOrderParametric(fg); reinit = false, @@ -979,16 +978,16 @@ function autoinitParametric!( 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, diff --git a/src/services/CalcFactor.jl b/src/services/CalcFactor.jl index a4abe244..b136f5d3 100644 --- a/src/services/CalcFactor.jl +++ b/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}, diff --git a/src/services/NumericalCalculations.jl b/src/services/NumericalCalculations.jl index 12668da1..91918bc0 100644 --- a/src/services/NumericalCalculations.jl +++ b/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 diff --git a/src/services/VariableStatistics.jl b/src/services/VariableStatistics.jl index e490a535..895f5c39 100644 --- a/src/services/VariableStatistics.jl +++ b/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 diff --git a/test/testBasicParametric.jl b/test/testBasicParametric.jl index 0144d32c..6a2afe70 100644 --- a/test/testBasicParametric.jl +++ b/test/testBasicParametric.jl @@ -8,16 +8,16 @@ using IncrementalInference @testset "Test consolidation of factors #467" begin fg = generateGraph_LineStep(20, poseEvery=1, landmarkEvery=4, posePriorsAt=collect(0:7), sightDistance=2, solverParams=SolverParams(algorithms=[:default, :parametric])) - d,st = IIF.solveGraphParametric(fg) - for i in 0:10 + M, labels, minimizer, Σ = IIF.solveGraphParametric(fg) + d = Dict(labels.=>minimizer) + for i in 0:20 sym = Symbol("x",i) - @test isapprox(d[sym].val[1], i, atol=1e-6) + @test isapprox(d[sym][1], i, atol=1e-6) end - d,st = IIF.solveGraphParametric(fg) - for i in 0:10 - sym = Symbol("x",i) - @test isapprox(d[sym].val[1], i, atol=1e-6) + for i in 0:4:20 + sym = Symbol("lm",i) + @test isapprox(d[sym][1], i, atol=1e-6) end end @@ -62,11 +62,12 @@ end ## fg = generateGraph_LineStep(7, poseEvery=1, landmarkEvery=0, posePriorsAt=collect(0:7), sightDistance=2, solverParams=SolverParams(algorithms=[:default, :parametric])) -d, result = IIF.solveGraphParametric(fg) +M, labels, minimizer, Σ = IIF.solveGraphParametric(fg) +d = Dict(labels.=>minimizer) for i in 0:7 sym = Symbol("x",i) - @test isapprox(d[sym].val[1], i, atol=1e-6) + @test isapprox(d[sym][1], i, atol=1e-6) end @@ -75,14 +76,14 @@ end fg = generateGraph_LineStep(2, graphinit=true, vardims=1, poseEvery=1, landmarkEvery=0, posePriorsAt=Int[0], sightDistance=3, solverParams=SolverParams(algorithms=[:default, :parametric])) r = IIF.autoinitParametric!(fg, :x0) -@test IIF.Optim.converged(r) +@test_broken IIF.Optim.converged(r) v0 = getVariable(fg,:x0) @test length(v0.solverDataDict[:parametric].val[1]) === 1 @test isapprox(v0.solverDataDict[:parametric].val[1][1], 0.0, atol = 1e-4) r = IIF.autoinitParametric!(fg, :x1) -@test IIF.Optim.converged(r) +@test_broken IIF.Optim.converged(r) v0 = getVariable(fg,:x1) @test length(v0.solverDataDict[:parametric].val[1]) === 1 @@ -108,12 +109,13 @@ fg = generateGraph_LineStep(10, vardims=2, poseEvery=1, landmarkEvery=3, posePri # foreach(fct->println(fct.label, ": ", getFactorType(fct).Z), getFactors(fg)) # @profiler d,st = IIF.solveGraphParametric(fg) -d,st = IIF.solveGraphParametric(fg) +M, labels, minimizer, Σ = IIF.solveGraphParametric(fg) +d = Dict(labels.=>minimizer) for i in 0:10 sym = Symbol("x",i) - @test isapprox(d[sym].val[1], i, atol=1e-6) - @test isapprox(d[sym].val[2], i, atol=1e-6) + @test isapprox(d[sym][1], i, atol=1e-6) + @test isapprox(d[sym][2], i, atol=1e-6) end # print results out @@ -124,7 +126,7 @@ end ## -foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second.val, pairs(d)) +foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second, pairs(d)) # getSolverParams(fg).dbg=true @@ -176,17 +178,18 @@ addFactor!(fg, [:x1; :x2], LinearRelative(Normal(0.0, 1e-1)), graphinit=graphini foreach(fct->println(fct.label, ": ", getFactorType(fct).Z), getFactors(fg)) -d,st,vs,Σ = IIF.solveGraphParametric(fg) +M, labels, minimizer, Σ = IIF.solveGraphParametric(fg) +d = Dict(labels.=>minimizer) foreach(println, d) -@test isapprox(d[:x0].val[1][1], -0.01, atol=1e-3) -@test isapprox(d[:x1].val[1][1], 0.0, atol=1e-3) -@test isapprox(d[:x2].val[1][1], 0.01, atol=1e-3) +@test isapprox(d[:x0][1][1], -0.01, atol=1e-3) +@test isapprox(d[:x1][1][1], 0.0, atol=1e-3) +@test isapprox(d[:x2][1][1], 0.01, atol=1e-3) ## -foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second.val, pairs(d)) +foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second, pairs(d)) # fg.solverParams.showtree = true # fg.solverParams.drawtree = true @@ -218,17 +221,17 @@ deleteFactor!(fg, :x5x6f1) # foreach(fct->println(fct.label, ": ", getFactorType(fct).Z), getFactors(fg)) # @profiler d,st = IIF.solveGraphParametric(fg) -d,st = IIF.solveGraphParametric(fg) - +M, labels, minimizer, Σ = IIF.solveGraphParametric(fg) +d = Dict(labels.=>minimizer) if false foreach(println, d) end for i in 0:10 sym = Symbol("x",i) - @test isapprox(d[sym].val[1], i, atol=1e-6) + @test isapprox(d[sym][1], i, atol=1e-6) end -foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second.val, pairs(d)) +foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second, pairs(d)) # fg.solverParams.showtree = true # fg.solverParams.drawtree = true