Custom Relative Factor

Define the relative struct

Previously we looked at making a Custom Prior Factor. This section describes how to build relative factors. Relative factors introduce relative-only information between variables in the factor graph, and do not add any absolute information. For example, a rigid transform between two variables is a relative relationship, regardless of their common absolute position in the world.

Lets look at either the EuclidDistance of Pose2Pose2 factors as simple examples. First, create the uniquely named factor struct:

struct EuclidDistance{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
  Z::T
end

New relative factors should either inheret from <:AbstractManifoldMinimize, <:AbstractRelativeMinimize, or <:AbstractRelativeRoots. These are all subtypes of <:AbstractRelative. There are only two abstract super types, <:AbstractPrior and <:AbstractRelative.

Summary of Sampling Data Representation

Specialized Dispatch (getManifold, getSample)

Relative factors involve computaton, these computations must be performed on some manifold. Custom relative factors require that the getManifold function be overridded. Here two examples are given for reference:

# import override/specialize the multiple dispatch
import DistributedFactorGraphs: getManifold

# two examples of existing functions in the standard libraries
DFG.getManifold(::EuclidDistance) = Manifolds.TranslationGroup(1)
DFG.getManifold(::Pose2Pose2) = Manifolds.SpecialEuclidean(2)

Extending the getSample method for our EuclidDistance factor example is not required, since the default dispatch using field .Z <: SamplableBelief will already be able to sample the measurement – see Specialized getSample.

One important note is that getSample for <:AbstractRelative factors should return measurement values as manifold tangent vectors – for computational efficiency reasons.

If more advanced sampling is required, extend the getSample function.

function getSample(cf::CalcFactor{<:Pose2Pose2}) 
  M = getManifold(cf.factor)
  ϵ = getPointIdentity(Pose2)
  X = sampleTangent(M, cf.factor.Z, ϵ)
  return X
end

The return type for getSample is unrestricted, and will be passed to the residual function "as-is", but must return values representing a tangent vector for <:AbstractRelative

Factor Residual Function

The selection of <:IIF.AbstractManifoldMinimize, akin to earlier <:AbstractPrior, instructs IIF to find the minimum of the provided residual function. The residual function is used during inference to approximate the convolution of conditional beliefs from the approximate beliefs of the connected variables in the factor graph. Conceptually, the residual function is usually something akin to residual = measurement - prediction, but does not have to follow the exact recipe.

The returned value (the factor measurement) from getSample will always be passed as the first argument (e.g. X) to the factor residual function.

# first residual function example
(cf::CalcFactor{<:EuclidDistance})(X, p, q) = X - norm(p .- q)

# second residual function example
function (cf::CalcFactor{<:Pose2Pose2})(X, p, q)
    M = getManifold(Pose2)
    q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X))
    Xc = vee(M, q, log(M, q, q̂))
    return Xc
end

It is recommended to leave the incoming types unrestricted. If you must define the types, make sure to allow sufficient dispatch freedom (i.e. dispatch to concrete types) and not force operations to "non-concrete" types. Usage can be very case specific, and hence better to let Julia type-inference automation do the hard work of inferring the concrete types.

Serialization

Serialization of factors is also discussed in more detail at Standardized Factor Serialization.