Merge pull request #1152 from JuliaRobotics/21Q1/fix/640

update approxConvBinary getSample, import Optim
JuliaRobotics · Feb 7, 2021 · c6a8457 · c6a8457
2 parents e7ce108 + 1624bab
commit c6a8457
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 16 deletions.
diff --git a/src/ApproxConv.jl b/src/ApproxConv.jl
@@ -631,7 +631,14 @@ function approxConvBinary(arr::Array{Float64,2},
 
   fmd.arrRef = t
 
-  measurement = size(measurement[1],2) == 0 ? sampleFactor(meas, N, fmd, vnds) : measurement
+  # TODO consolidate with ccwl??
+  # FIXME do not divert Mixture for sampling
+  # cf = _buildCalcFactorMixture(ccwl, fmd, 1, ccwl.measurement, ARR) # TODO perhaps 0 is safer
+  # FIXME 0, 0, ()
+  cf = CalcFactor( meas, fmd, 0, 0, (), t)
+
+  measurement = size(measurement[1],2) == 0 ? sampleFactor(cf, N) : measurement
+  # measurement = size(measurement[1],2) == 0 ? sampleFactor(meas, N, fmd, vnds) : measurement
 
   zDim = size(measurement[1],1)
   ccw = CommonConvWrapper(meas, t[varidx], zDim, t, fmd, varidx=varidx, measurement=measurement)  # N=> size(measurement[1],2)

diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl
@@ -12,16 +12,17 @@ using Reexport
 # @reexport using Graphs
 @reexport using LinearAlgebra
 
+import NLsolve
+import NLSolversBase
+import Optim
+
 using
   Dates,
   TimeZones,
   DistributedFactorGraphs,
   DelimitedFiles,
   Statistics,
   Random,
-  NLsolve,
-  NLSolversBase,
-  Optim,
   StatsBase,
   JLD2,
   BSON,

diff --git a/src/NumericalCalculations.jl b/src/NumericalCalculations.jl
@@ -43,7 +43,7 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
   #
 
   #
-  r = nlsolve( (res, x) -> res .= objResX(x), u0, inplace=true)
+  r = NLsolve.nlsolve( (res, x) -> res .= objResX(x), u0, inplace=true)
 
   #
   return r.zero
@@ -58,9 +58,9 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
   #
   # wrt #467 allow residual to be standardize for Roots and Minimize and Parametric cases.
   r = if islen1
-    optimize((x) -> (residual .= objResX(x); sum(residual.^2)), u0, BFGS() )
+    Optim.optimize((x) -> (residual .= objResX(x); sum(residual.^2)), u0, Optim.BFGS() )
   else
-    optimize((x) -> (residual .= objResX(x); sum(residual.^2)), u0)
+    Optim.optimize((x) -> (residual .= objResX(x); sum(residual.^2)), u0)
   end
 
   # 

diff --git a/src/ParametricUtils.jl b/src/ParametricUtils.jl
@@ -173,7 +173,7 @@ function solveFactorGraphParametric(fg::AbstractDFG;
                                     useCalcFactor::Bool=true, #TODO dev param will be removed
                                     solvekey::Symbol=:parametric,
                                     autodiff = :forward,
-                                    algorithm=BFGS,
+                                    algorithm=Optim.Optim.BFGS,
                                     algorithmkwargs=(), # add manifold to overwrite computed one
                                     options = Optim.Options(allow_f_increases=true,
                                                             time_limit = 100,
@@ -209,15 +209,15 @@ function solveFactorGraphParametric(fg::AbstractDFG;
 
   if useCalcFactor
     cfd = calcFactorMahalanobisDict(fg)
-    tdtotalCost = TwiceDifferentiable((x)->_totalCost(cfd, flatvar, x), initValues, autodiff = autodiff)
+    tdtotalCost = Optim.TwiceDifferentiable((x)->_totalCost(cfd, flatvar, x), initValues, autodiff = autodiff)
   else
-    tdtotalCost = TwiceDifferentiable((x)->_totalCost(fg, flatvar, x), initValues, autodiff = autodiff)
+    tdtotalCost = Optim.TwiceDifferentiable((x)->_totalCost(fg, flatvar, x), initValues, autodiff = autodiff)
   end
 
-  result = optimize(tdtotalCost, initValues, alg, options)
+  result = Optim.optimize(tdtotalCost, initValues, alg, options)
   rv = Optim.minimizer(result)
 
-  H = hessian!(tdtotalCost, rv)
+  H = Optim.hessian!(tdtotalCost, rv)
 
   Σ = pinv(H)
 
@@ -241,7 +241,7 @@ function solveConditionalsParametric(fg::AbstractDFG,
                                     frontals::Vector{Symbol};
                                     solvekey::Symbol=:parametric,
                                     autodiff = :forward,
-                                    algorithm=BFGS,
+                                    algorithm=Optim.BFGS,
                                     algorithmkwargs=(), # add manifold to overwrite computed one
                                     options = Optim.Options(allow_f_increases=true,
                                                             time_limit = 100,
@@ -276,14 +276,14 @@ function solveConditionalsParametric(fg::AbstractDFG,
   mc_mani = MixedCircular(fg, varIds)
   alg = algorithm(;manifold=mc_mani, algorithmkwargs...)
 
-  tdtotalCost = TwiceDifferentiable((x)->_totalCost(fg, flatvar,x), initValues, autodiff = autodiff)
+  tdtotalCost = Optim.TwiceDifferentiable((x)->_totalCost(fg, flatvar,x), initValues, autodiff = autodiff)
 
-  result = optimize((x)->_totalCost(fg, flatvar, [x;sX]), fX, alg, options)
+  result = Optim.optimize((x)->_totalCost(fg, flatvar, [x;sX]), fX, alg, options)
   # result = optimize(x->totalCost([x;sX]), fX, alg, options)
 
   rv = Optim.minimizer(result)
 
-  H = hessian!(tdtotalCost, [rv; sX])
+  H = Optim.hessian!(tdtotalCost, [rv; sX])
 
   Σ = pinv(H)