Parametric Solve (Experimental)

Note that parametric solve (i.e. conventional Gaussians) is currently supported as an experimental feature which might appear more buggy. Familiar parametric methods should become fully integrated and we invite comments or contributions from the community. A great deal of effort has gone into finding the best abstractions to support multiple factor graph solving strategies.

Batch Parametric

IncrementalInference.solveGraphParametric — Function

solveGraphParametric(args; kwargs...)

Batch parametric graph solve using Riemannian Levenberg Marquardt.

source

DistributedFactorGraphs.solveGraphParametric! — Function

Standard parametric graph solution (Experimental).

source

Initializing the parametric solve from existing values can be done with the help of:

IncrementalInference.initParametricFrom! — Function

initParametricFrom!(fg; ...)
initParametricFrom!(fg, fromkey; parkey, onepoint, force)

Initialize the parametric solver data from a different solution in fromkey.

DevNotes

TODO, keyword force not wired up yet.

source

Defining Factors to Support a Parametric Solution (Experimental)

Factor that supports a parametric solution, with supported distributions (such as Normal and MvNormal), can be used in a parametric batch solver solveGraphParametric.

`getMeasurementParametric`

Parameteric calculations require the mean and covariance from Gaussian measurement functions (factors) using the getMeasurementParametric getMeasurementParametric defaults to looking for a supported distribution in field .Z followed by .z. Therefore, if the factor uses this fieldname, getMeasurementParametric does not need to be extended. You can extend by simply implementing, for example, your own IncrementalInference.getMeasurementParametric(f::OtherFactor) = m.density.

For this example, the Z field will automatically be detected used by default for MyFactor from above.

struct MyFactor{T <: SamplableBelief} <: IIF.AbstractRelativeRoots
  Z::T
end

An example of where implementing getMeasurementParametric is needed can be found in the RoME factor Pose2Point2BearingRange

import getMeasurementParametric
function getMeasurementParametric(s::Pose2Point2BearingRange{<:Normal, <:Normal})

  meas = [mean(s.bearing), mean(s.range)]
  iΣ = [1/var(s.bearing)             0;
                      0  1/var(s.range)]

  return meas, iΣ
end

The Factor

The factor is evaluated in a cost function using the Mahalanobis distance and the measurement should therefore match the residual returned.

Optimization

IncrementalInference.solveGraphParametric! uses Optim.jl. The factors that are supported should have a gradient and Hessian available/exists and therefore it makes use of TwiceDifferentiable. Full control of Optim's setup is possible with keyword arguments.