JuliaRobotics / IncrementalInference.jl

@@ -508,8 +508,14 @@
Loading
508 508
  computeCovariance::Bool = true,
509 509
  solveKey::Symbol = :parametric,
510 510
  autodiff = :forward,
511 -
  algorithm = Optim.BFGS,
512 -
  algorithmkwargs = (), # add manifold to overwrite computed one
511 +
  algorithm = Optim.NewtonTrustRegion, # Optim.BFGS,
512 +
  algorithmkwargs = (
513 +
        initial_delta = 1.0,
514 +
        # delta_hat = 1.0,
515 +
        eta = 0.01,
516 +
        # rho_lower = 0.25,
517 +
        # rho_upper = 0.75
518 +
  ), # add manifold to overwrite computed one
513 519
  options = Optim.Options(;
514 520
    allow_f_increases = true,
515 521
    time_limit = 100,
@@ -542,6 +548,22 @@
Loading
542 548
543 549
  #optim setup and solve
544 550
  alg = algorithm(; algorithmkwargs...)
551 +
  # alg = NewtonTrustRegion(;
552 +
  #   initial_delta = 1.0,
553 +
  #   delta_hat = 100.0,
554 +
  #   eta = 0.1,
555 +
  #   rho_lower = 0.25,
556 +
  #   rho_upper = 0.75
557 +
  # )
558 +
  # alg = LBFGS(; 
559 +
  #   m = 10,
560 +
  #   alphaguess = LineSearches.InitialStatic(),
561 +
  #   linesearch = LineSearches.HagerZhang(),
562 +
  #   P = nothing,
563 +
  #   precondprep = (P, x) -> nothing,
564 +
  #   manifold = Flat(),
565 +
  #   scaleinvH0::Bool = true && (typeof(P) <: Nothing)
566 +
  # )
545 567
  tdtotalCost = Optim.TwiceDifferentiable(gsc, initValues; autodiff = autodiff)
546 568
547 569
  result = Optim.optimize(tdtotalCost, initValues, alg, options)
@@ -791,11 +813,19 @@
Loading
791 813
    $SIGNATURES
792 814
Add parametric solver to fg, batch solve using [`solveGraphParametric`](@ref) and update fg.
793 815
"""
794 -
function solveGraphParametric!(fg::AbstractDFG; init::Bool = true, kwargs...)
816 +
function solveGraphParametric!(
817 +
  fg::AbstractDFG; 
818 +
  init::Bool = true, 
819 +
  solveKey::Symbol = :parametric, # FIXME, moot since only :parametric used for parametric solves
820 +
  initSolveKey::Symbol = :default, 
821 +
  kwargs...
822 +
)
823 +
  # make sure variables has solverData, see #1637
824 +
  makeSolverData!(fg; solveKey)
795 825
  if !(:parametric in fg.solverParams.algorithms)
796 826
    addParametricSolver!(fg; init = init)
797 827
  elseif init
798 -
    initParametricFrom!(fg)
828 +
    initParametricFrom!(fg, initSolveKey; parkey=solveKey)
799 829
  end
800 830
801 831
  vardict, result, varIds, Σ = solveGraphParametric(fg; kwargs...)

@@ -364,7 +364,7 @@
Loading
364 364
    @info "Setting `.multiproc=false` since `Distributed.nprocs() == 1`"
365 365
    opt.multiproc = false
366 366
  end
367 -
367 +
  
368 368
  if opt.graphinit
369 369
    @info "Ensure variables are all initialized (graphinit)"
370 370
    if algorithm == :parametric

@@ -362,14 +362,15 @@
Loading
362 362
  return resetVariable!(getVariable(dfg, sym); solveKey = solveKey)
363 363
end
364 364
365 +
# return VariableNodeData
365 366
function DefaultNodeDataParametric(
366 367
  dodims::Int,
367 368
  dims::Int,
368 369
  variableType::InferenceVariable;
369 370
  initialized::Bool = true,
370 371
  dontmargin::Bool = false,
371 -
)::VariableNodeData
372 -
372 +
  solveKey::Symbol = :parametric
373 +
)
373 374
  # this should be the only function allocating memory for the node points
374 375
  if false && initialized
375 376
    error("not implemented yet")
@@ -408,14 +409,23 @@
Loading
408 409
  end
409 410
end
410 411
412 +
"""
413 +
    $SIGNATURES
414 +
415 +
Makes and sets a parametric `VariableNodeData` object (`.solverData`).
416 +
417 +
DevNotes
418 +
- TODO assumes parametric solves will always just be under the `solveKey=:parametric`, should be generalized.
419 +
"""
411 420
function setDefaultNodeDataParametric!(
412 421
  v::DFGVariable,
413 422
  variableType::InferenceVariable;
423 +
  solveKey::Symbol = :parametric,
414 424
  kwargs...,
415 425
)
416 -
  vnd = DefaultNodeDataParametric(0, variableType |> getDimension, variableType; kwargs...)
417 -
  setSolverData!(v, vnd, :parametric)
418 -
  return nothing
426 +
  vnd = DefaultNodeDataParametric(0, variableType |> getDimension, variableType; solveKey, kwargs...)
427 +
  setSolverData!(v, vnd, solveKey)
428 +
  nothing
419 429
end
420 430
421 431
"""

@@ -101,6 +101,7 @@
Loading
101 101
# API Exports
102 102
103 103
# Package aliases
104 +
# FIXME, remove this and let the user do either import or const definitions
104 105
export KDE, AMP, DFG, FSM, IIF
105 106
106 107
# TODO temporary for initial version of on-manifold products

@@ -5,28 +5,36 @@
Loading
5 5
6 6
For variables in `varList` check and if necessary make solverData objects for both `:default` and `:parametric` solveKeys. 
7 7
8 +
Example
9 +
```julia
10 +
num_made = makeSolverData(fg; solveKey=:parametric)
11 +
```
12 +
8 13
Notes
9 14
- Part of solving JuliaRobotics/IncrementalInference.jl issue 1637
10 15
11 -
See also: [`doautoinit!`](@ref)
16 +
DevNotes
17 +
- TODO, assumes parametric solves will always just be in solveKey `:parametric`.
18 +
19 +
See also: [`doautoinit!`](@ref), [`initAll!`](@ref)
12 20
"""
13 21
function makeSolverData!(
14 22
  dfg::AbstractDFG;
15 23
  solvable = 1,
16 -
  varList::AbstractVector{Symbol} = ls(dfg; solvable)
24 +
  varList::AbstractVector{Symbol} = ls(dfg; solvable),
25 +
  solveKey::Symbol=:default
17 26
)
18 27
  count = 0
19 28
  for vl in varList
20 29
    v = getVariable(dfg,vl)
21 30
    varType = getVariableType(v) |> IIF._variableType
22 31
    vsolveKeys = listSolveKeys(dfg,vl)
23 -
    if !(:default in vsolveKeys)
24 -
        IIF.setDefaultNodeData!(v, 0, getSolverParams(dfg).N, getDimension(varType); initialized=false, varType) # dodims
32 +
    if solveKey != :parametric && !(solveKey in vsolveKeys)
33 +
        IIF.setDefaultNodeData!(v, 0, getSolverParams(dfg).N, getDimension(varType); initialized=false, varType, solveKey) # dodims
25 34
        count += 1
26 -
    end
27 -
    if !(:parametric in vsolveKeys)
35 +
    elseif solveKey == :parametric && !(:parametric in vsolveKeys)
28 36
        # global doinit = true
29 -
        IIF.setDefaultNodeDataParametric!(v, varType, initialized=false)
37 +
        IIF.setDefaultNodeDataParametric!(v, varType; initialized=false, solveKey)
30 38
        count += 1
31 39
    end
32 40
  end
@@ -499,7 +507,8 @@
Loading
499 507
    varType = getVariableType(vari) |> _variableType
500 508
    # does SolverData exist for this solveKey?
501 509
    vsolveKeys = listSolveKeys(vari)
502 -
    if !_parametricInit && !(solveKey in vsolveKeys)
510 +
    # FIXME, likely some consolidation needed with #1637
511 +
    if !_parametricInit && !(solveKey in vsolveKeys)  
503 512
      # accept complete defaults for a novel solveKey
504 513
      setDefaultNodeData!(
505 514
        vari,
Files Coverage
src 67.44%
Project Totals (72 files) 67.44%
Sunburst
The inner-most circle is the entire project, moving away from the center are folders then, finally, a single file. The size and color of each slice is representing the number of statements and the coverage, respectively.
Icicle
The top section represents the entire project. Proceeding with folders and finally individual files. The size and color of each slice is representing the number of statements and the coverage, respectively.
Grid
Each block represents a single file in the project. The size and color of each block is represented by the number of statements and the coverage, respectively.
Loading