Commit ⋅ JuliaRobotics/IncrementalInference.jl

Showing 5 of 5 files from the diff.

47.79% 0.00% -21.22%

src/ParametricUtils.jl changed.

89.32% ø ø

src/SolverAPI.jl changed.

72.52% 100.00% ø

src/services/FactorGraph.jl changed.

87.50% ø ø

src/IncrementalInference.jl changed.

88.13% 60.00% -3.13%

src/services/GraphInit.jl changed.

@@ -508,8 +508,14 @@

  computeCovariance::Bool = true,
  solveKey::Symbol = :parametric,
  autodiff = :forward,
  algorithm = Optim.BFGS,
  algorithmkwargs = (), # add manifold to overwrite computed one
  algorithm = Optim.NewtonTrustRegion, # Optim.BFGS,
  algorithmkwargs = (
        initial_delta = 1.0,
        # delta_hat = 1.0,
        eta = 0.01,
        # rho_lower = 0.25,
        # rho_upper = 0.75
  ), # add manifold to overwrite computed one
  options = Optim.Options(;
    allow_f_increases = true,
    time_limit = 100,

@@ -542,6 +548,22 @@


  #optim setup and solve
  alg = algorithm(; algorithmkwargs...)
  # alg = NewtonTrustRegion(;
  #   initial_delta = 1.0,
  #   delta_hat = 100.0,
  #   eta = 0.1,
  #   rho_lower = 0.25,
  #   rho_upper = 0.75
  # )
  # alg = LBFGS(; 
  #   m = 10,
  #   alphaguess = LineSearches.InitialStatic(),
  #   linesearch = LineSearches.HagerZhang(),
  #   P = nothing,
  #   precondprep = (P, x) -> nothing,
  #   manifold = Flat(),
  #   scaleinvH0::Bool = true && (typeof(P) <: Nothing)
  # )
  tdtotalCost = Optim.TwiceDifferentiable(gsc, initValues; autodiff = autodiff)

  result = Optim.optimize(tdtotalCost, initValues, alg, options)

@@ -791,11 +813,19 @@

    $SIGNATURES
Add parametric solver to fg, batch solve using [`solveGraphParametric`](@ref) and update fg.
"""
function solveGraphParametric!(fg::AbstractDFG; init::Bool = true, kwargs...)
function solveGraphParametric!(
  fg::AbstractDFG; 
  init::Bool = true, 
  solveKey::Symbol = :parametric, # FIXME, moot since only :parametric used for parametric solves
  initSolveKey::Symbol = :default, 
  kwargs...
)
  # make sure variables has solverData, see #1637
  makeSolverData!(fg; solveKey)
  if !(:parametric in fg.solverParams.algorithms)
    addParametricSolver!(fg; init = init)
  elseif init
    initParametricFrom!(fg)
    initParametricFrom!(fg, initSolveKey; parkey=solveKey)
  end

  vardict, result, varIds, Σ = solveGraphParametric(fg; kwargs...)

@@ -364,7 +364,7 @@

    @info "Setting `.multiproc=false` since `Distributed.nprocs() == 1`"
    opt.multiproc = false
  end

  
  if opt.graphinit
    @info "Ensure variables are all initialized (graphinit)"
    if algorithm == :parametric

@@ -362,14 +362,15 @@

  return resetVariable!(getVariable(dfg, sym); solveKey = solveKey)
end

# return VariableNodeData
function DefaultNodeDataParametric(
  dodims::Int,
  dims::Int,
  variableType::InferenceVariable;
  initialized::Bool = true,
  dontmargin::Bool = false,
)::VariableNodeData

  solveKey::Symbol = :parametric
)
  # this should be the only function allocating memory for the node points
  if false && initialized
    error("not implemented yet")

@@ -408,14 +409,23 @@

  end
end

"""
    $SIGNATURES

Makes and sets a parametric `VariableNodeData` object (`.solverData`).

DevNotes
- TODO assumes parametric solves will always just be under the `solveKey=:parametric`, should be generalized.
"""
function setDefaultNodeDataParametric!(
  v::DFGVariable,
  variableType::InferenceVariable;
  solveKey::Symbol = :parametric,
  kwargs...,
)
  vnd = DefaultNodeDataParametric(0, variableType |> getDimension, variableType; kwargs...)
  setSolverData!(v, vnd, :parametric)
  return nothing
  vnd = DefaultNodeDataParametric(0, variableType |> getDimension, variableType; solveKey, kwargs...)
  setSolverData!(v, vnd, solveKey)
  nothing
end

"""

@@ -101,6 +101,7 @@

# API Exports

# Package aliases
# FIXME, remove this and let the user do either import or const definitions
export KDE, AMP, DFG, FSM, IIF

# TODO temporary for initial version of on-manifold products

@@ -5,28 +5,36 @@


For variables in `varList` check and if necessary make solverData objects for both `:default` and `:parametric` solveKeys. 

Example
```julia
num_made = makeSolverData(fg; solveKey=:parametric)
```

Notes
- Part of solving JuliaRobotics/IncrementalInference.jl issue 1637

See also: [`doautoinit!`](@ref)
DevNotes
- TODO, assumes parametric solves will always just be in solveKey `:parametric`.

See also: [`doautoinit!`](@ref), [`initAll!`](@ref)
"""
function makeSolverData!(
  dfg::AbstractDFG;
  solvable = 1,
  varList::AbstractVector{Symbol} = ls(dfg; solvable)
  varList::AbstractVector{Symbol} = ls(dfg; solvable),
  solveKey::Symbol=:default
)
  count = 0
  for vl in varList
    v = getVariable(dfg,vl)
    varType = getVariableType(v) |> IIF._variableType
    vsolveKeys = listSolveKeys(dfg,vl)
    if !(:default in vsolveKeys)
        IIF.setDefaultNodeData!(v, 0, getSolverParams(dfg).N, getDimension(varType); initialized=false, varType) # dodims
    if solveKey != :parametric && !(solveKey in vsolveKeys)
        IIF.setDefaultNodeData!(v, 0, getSolverParams(dfg).N, getDimension(varType); initialized=false, varType, solveKey) # dodims
        count += 1
    end
    if !(:parametric in vsolveKeys)
    elseif solveKey == :parametric && !(:parametric in vsolveKeys)
        # global doinit = true
        IIF.setDefaultNodeDataParametric!(v, varType, initialized=false)
        IIF.setDefaultNodeDataParametric!(v, varType; initialized=false, solveKey)
        count += 1
    end
  end

@@ -499,7 +507,8 @@

    varType = getVariableType(vari) |> _variableType
    # does SolverData exist for this solveKey?
    vsolveKeys = listSolveKeys(vari)
    if !_parametricInit && !(solveKey in vsolveKeys)
    # FIXME, likely some consolidation needed with #1637
    if !_parametricInit && !(solveKey in vsolveKeys)  
      # accept complete defaults for a novel solveKey
      setDefaultNodeData!(
        vari,

Files		•	•	•	Coverage
src	5,592	3,771	0	1,821	67.44%
Project Totals (72 files)	5,592	3,771	0	1,821	67.44%

View CI Build Download

No yaml found.

Create your codecov.yml to customize your Codecov experience

Read the docs

Interact now

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.

SVG

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.

SVG

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.

SVG

Learn more about the coverage sunburst graph

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	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	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	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	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	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	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	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	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,

Navigation	Overlay
`t` Navigate files	`h` Toggle hits
`y` Change url to tip of branch	`m` Toggle misses
`b / v` Jump to prev/next hit line	`p` Toggle partial
`z / x` Jump to prev/next missed or partial line	`1..9` Toggle flags
`shift + o` Open current page in GitHub	`a` Toggle all on
`/ or ?` Show keyboard shortcuts dialog	`c` Toggle context lines or commits

Updating our web app

No yaml found.