improve non-parametric performance

JuliaRobotics · Oct 12, 2023 · bbed441 · bbed441
1 parent 31ce516
commit bbed441
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 17 deletions.
diff --git 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/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
@@ -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