Consolidate / Simplify factor evaluation (CalcFactor)

[dehann]

EDITED (20Q4 / 21Q1):

Using this issue for topics related to consolidating the nonparametric and parametric factor residual function API. Idea is user writes one residual factor in a sensible way, and IIF is able to use that factor for both parametric and nonparametric with little to no change from the user perspective. A critical point to note here is that nonparametric (nonGaussian) is much more versatile and general, so there are many many cases where parametric will just not be possible -- regardless the API should be standardized.

References:

• Similar for IIF.getSample standardize special sampler using new FMd / CalcResidual #927
• Adjacent issue Bug, FMD not doing multihypo correctly #1025
• TODO docs Document how to use CF data in factor evaluation #1044
• Towards closing Integrate with Manifolds.jl RoME.jl#244
  • Keeping in mind: Ball/Metric Tree using NearestNeighbors.jl / Manifolds.jl / Distances.jl / CoordinateTransformations.jl ApproxManifoldProducts.jl#41 (comment)

---

Current holdups:

• Consolidate / Simplify factor evaluation (CalcFactor) #467 (comment)
• Nonparametric AbstractRelativeMinimize should return residual::Vector, not self accumulate to a Real scalar value.

---

New Documentation

• https://juliarobotics.org/Caesar.jl/latest/examples/basic_definingfactors/

---

Preliminary New API Example

• also see Consolidate / Simplify factor evaluation (CalcFactor) #467 (comment)

IncrementalInference.jl/src/Factors/LinearRelative.jl

Lines 42 to 51 in 0469062

	# new and simplified interface for both nonparametric and parametric
	function (s::CalcFactor{<:LinearRelative})( res::AbstractVector{<:Real},
	z,
	x1,
	x2 ) # where {M<:FactorMetadata,P<:Tuple,X<:AbstractVector}
	#
	# TODO convert to distance(distance(x2,x1),z) # or use dispatch on `-` -- what to do about `.-`
	res .= z - (x2 - x1)
	nothing
	end

---

Parametric Integration Work 90% (?) Done

DF, I was hoping to just finish the consolidation with (#467 (comment)) but alas! Note that much of the consolidation work has already been done surrounding _CalcFactorParametric as the internal interface between new CalcFactor API and solve__Parametric functions. E.g. see:

IncrementalInference.jl/src/ParametricUtils.jl

Lines 81 to 90 in 9e225b7

	# pass in residual for consolidation with nonparametric
	# userdata is now at `cfp.cf.cachedata`
	function (cfp::_CalcFactorParametric)(variables...)
	# call the user function (be careful to call the new CalcFactor version only!!!)
	res = zeros(length(cfp.meanVal))
	cfp.calcfactor!(res, cfp.meanVal, variables...)
	# 1/2*log(1/( sqrt(det(Σ)*(2pi)^k) )) ## k = dim(μ)
	return 0.5 * (res' * cfp.informationMat * res)
	end

---

Original Issue Question

How would you use a normal IIF factor in parametric? example.

[dehann]

cc @Affie

[Affie]

EDIT, DF this parametric only API is being deprecated/removed.

---

Ok to implement cost directly

function (s::LinearConditional)(X1::Vector{Float64},
                                X2::Vector{Float64};
                                userdata::Union{Nothing,FactorMetadata}=nothing)
  # getFactorMean(s)
  meas = s.Z.μ
  # getFactorCov(s)
  σ = s.Z.σ
  res = [meas - (X2[1] - X1[1])]
  return (res/σ) .^ 2
end

[dehann]

Unfortunately i don't think we should re-implement the costs as duplicates -- okay for dev just to get started, but would rather do this with a wrapper over the existing factor definitions. I can help with that and is on my TODO list.

In the mean time see common reference here:

• https://juliarobotics.org/Caesar.jl/latest/examples/adding_variables_factors/
• https://www.juliarobotics.org/Caesar.jl/latest/examples/basic_definingfactors/

[Affie]

I've temporary continued with the re-implementing approach for simplicity.
It looks like a wrapper can potentially work on all factors looked at except Pose2Point2BearingRange

[dehann]

We can sort out the wrappers for dual nonparametric parametric use in IIF v0.9.x (should finally be done by v0.21, was the most sensible, albeit longer, path)

EDITED

[dehann]

will resolve this after #459 is completed. Only down message consolidation remains at this point.

[dehann]

FMD refactor must be completed first

EDIT: roughly speaking, see #1025

[dehann]

Hi @Affie , thanks -- there is a lot to unpack here. Think this is in the right ball park with some good but maybe a few pieces of this may have to change... I want to be very careful about communicating accurately here else we going to go around in circles. Give me a few hours to write clear (so-called "counter") examples.

maybe added complexity of giving users the ability to create 2 factor types?

This is the part I'm most worried about (big 'no' from me unless you mean something specific). Lets make sure we are talking the same language here and forthcoming "counter" examples is probably best -- stay tuned...

[dehann]

N.B. this a pointed technical discussion to try understand a far reaching multiple-dispatch issue -- no harm intended. I don't know if any other hard-typed parametric and nonparametric libraries exist that can do both styles of inference using exactly the same member definitions as we are doing here.

( The issue at hand is to use standard Julia autodiff tools while maintaining statically typed functions for all run-time, user defined factor types, including automated blends of Mixtures, multihypo=, nullhypo=, and access to advanced data management. The user does not necessarily realize that all this comes from their single member definition. Hence my drive below to constrain to a single but powerful API.)

Never do this

function (s::CalcFactor)(variables...) 
  s.res .= s.factor(variables...)
  return sum(s.resl.^2) 
end

or

# then the default call for CalcFactor for non-parametric
function (s::CalcFactor)(variables...) 
  s.res .= s.factor(variables...)
  nothing
end

Specifically the part s.factor(variables...), since that totally destroys any attempt of standardizing the API. It's a quick fix for small/easy problems, but has no support for all the hard problems we have been facing for years. So this cannot be used:

#The easy/basic factor function definitions
function (s__::LinearRelative)(z,x1,x2)
  res = z - (x2 - x1)
end

Because it is super important that users learn / use only ONE SINGLE API definition that works for all cases, that is (s::CalcFactor{<:MyFactor})(z1, z2, x1,x2,x3,x4,...). There is far too much history of wanting to get metadata, smalldata, entry=>data, timestamps, or needing to deal with multihypo=, or needing .specialSampler, etc. etc. etc. So I think for the short-medium term this will be an absolute requirement. My concern is that Gaussian parametric only super-oversimplifies the problem which leads us away from solving the "hard" problem (i.e. non-Gaussian). The practical cost of this is a standardized API based around CalcFactor, and best practice is maintain one standard throughout. (think that is the fundamental issue we trying to figure out here).

Also remember there are also years of pain baked into getSample(s::CalcFactor{<:MyFactor}, N) which is not apparent in this residual factor write-up, but has same arguments.

Notice that the existing code forces us to use both NLsolve.jl and Optim.jl, which allows both AbstractRelativeRoots (more efficient) and AbstractRelativeMinimize (more flexible but slower) options. Whatever we do here in 467 must maintain this requirement.

Okay, so @Affie are you saying that this is not a workable solution for consolidated ("Malahanobis distance") evaluation?

IncrementalInference.jl/src/NumericalCalculations.jl

Line 61 in 6ddd346

optimize((x) -> (objResX(residual, x); sum(residual.^2)), u0, BFGS() )

---

Should Standardize to Only This

#The standard factor function definition template
function (s::CalcFactor{<:MyFactor})(measurements..., variables...) 
  # maybe you need other stuff...
  # t1 = getTimestamp(s.metadata.dfgVariable[1])
  
  # maybe inline fails and you can avoid memory allocations by reusing
  # s.residual[1] = ... # some compute A
 
  # or you directly use your own internal
  res1 = ... # some compute A
  res2 = ... # more compute B

  # alternatively user is forcing in-place and does,  return s.residual
  return [res1; res2]  
end

This means we would do something like:

(s::CalcFactor{<:LinearRelative})(z,x1,x2) = z - (x2 - x1)

I really do not see a problem with using this later as a simple single standard that works for all known use-cases.

Are you saying @Affie there is absolutely no way in which you can get the type-stable dispatch to work (including todays fix #1138 ) using this later definition for both nonparametric and parametric?

Parametric should then be able to just use this for Malahanobis distance calculations (can do pseudo-Huber cost function, etc. later):

IncrementalInference.jl/src/ParametricUtils.jl

Lines 103 to 106 in d06fe4c

	res = cfp.calcfactor!(cfp.meas, variables...)
	# 1/2*log(1/( sqrt(det(Σ)*(2pi)^k) )) ## k = dim(μ)
	return 0.5 * (res' * iΣ * res)

Also remember we will also do max-mixtures by consolidating parametric with Mixture -- that is a whole other dispatch party (see #931).

Finally, IIF v0.21.0 will likely again bring a breaking change on factor residual API, so that the residual function always returns residual (or res). This is in contrast to current v0.20.0 format where factors always return nothing.

[Affie]

The "returning residual" way

Are you saying @Affie there is absolutely no way in which you can get the type-stable dispatch to work (including todays fix #1138 ) using this later definition for both nonparametric and parametric?

Not at all, this is the quickest and easiest way to get type stability, leave it up to julia to figure out the return type. I'm 100% happy with it. I started implementing it but got stuck on non-parametric that calls the function with the residual as a parameter and the difference that is then required between Minimize and Roots (as far as I understand).

The "never do this" way

I'm not sure I follow your answer and if my example was clear enough, but it will break your ONE SINGLE API rule for factor definitions as there will be one for the simple (<:FunctorInferenceType) and one for complex (<:CalcFactor{<:FunctorInferenceType}) factor types.

[Affie]

Perhaps we can get away with #1140. It still requires proper testing in RoME though.

[dehann]

RE: returning the residual, "Not at all, this is the quickest and easiest way to get type stability, leave it up to julia to figure out the return type. I'm 100% happy with it."

That's great so we have consensus on that part! I strongly agree with leave it up to julia to figure out the return type

[dehann]

I'm not sure I follow your answer and if my example was clear enough

I think I follow your example and if so, I'm trying to stop you from going down the path of creating an "easy" and "complex" interface. I am very strongly against that idea. Thats what I meant with "never do this way". (Going back at least 14 months, #467 (comment))

but it will break your ONE SINGLE API rule for factor definitions as there will be one for the simple (<:FunctorInferenceType) and one for complex (<:CalcFactor{<:FunctorInferenceType}) factor types.

This is what I'm trying to avoid and stop. Let me look at 1140 and see if I can say anything more useful. But just note, at this stage I would not merge anything on factors that breaks the "one API only requirement".

[dehann]

With #1140 as a 'towards' consolidation it looks fine. I know the experimental parametric factor definitions follow the "easy" interface that still excludes the new CalcFactor API approach (#467 (comment)). The reason for this 467 is to consolidate down to only one single API, hence we have to do away with the idea of an "easy" and "complex" interface. The logic shall from here on out just be "one API, that is as simple as possible, and CalcFactor looks like the best way to do it".

To illustrate again, I really cannot see what the 'complexity' is with a definition like this (and note this does satisfy the "one api requirement"):

(s::CalcFactor{<:LinearRelative})(z, x1,x2) = z - (x2 - x1)

[dehann]

Just to be clear, we have a project wide requirement that there should be just one API for residual factor evaluation. In the future there will be multiple algorithms and our history of using these factors in different applications FORCES us to create the headroom via CalcFactor. No point in using Julia if we are manually writing factors for each dispatch case, and furthermore way way way too confusing when new users are trying to learn the library.

This one API is now definitely an absolute requirement for the foreseeable future. Sorry for pushing so hard on this, but let's just get it done and keep moving.

EDIT, added this to the high level requirements list:
https://github.com/JuliaRobotics/Caesar.jl/wiki/High-Level-Requirements

[Affie]

I'm trying to stop you from going down the path of creating an "easy" and "complex" interface

I just wrote down an idea to enable factor reuse. It would still all be CalcFactor, but you can re-use the simple definitions in CalcFactor I don't know if it would ever be needed though.
It's not something I looked at more than writing it down. Also not related to 1140, see next comment on it.

[Affie]

Regarding #1140, I merged it as it can use CalcFactor as is (I still have to be tested in RoME though). The "return nothing" way.

The next step with parametric is to update RoME for Parametric CalcFactor and properly test it with #1140.

We should decide if factors should return residual or change parameter and return nothing.

• It might be easier with manifold integration to return the residual.
• Returning Nothing makes it harder for parametric to know the return type beforehand to pass in as a parameter. I don't know if the hack to always use the element type of the first argument will hold.

  IncrementalInference.jl/src/ParametricUtils.jl

  Line 96 in 85bf682

  res = Vector{eltype(variables[1])}(undef, length(cfp.meas))

[dehann]

ping @Affie

We should decide if factors should return residual or change parameter and return nothing

Sorry if that has not been clear. Yes please always return the residual for both Roots and Minimize. Following from our discussions, this sounds like the best direction for new single API for factor residual functions (to solve dispatch on autodiff). And we are removing res from the parameter list, which will still be available (if user wants/needs to use) via reference at CalcFactor.residual.

I don't know if the hack to always use the element type of the first argument will hold.

I doubt this will hold for very long, but lets get the consolidation done on actual functions and then worry about generalizing residual type for more cases.

The main task item right now is droping two factor definitions between different algorithms, down to only the one single API usage.

[dehann]

Hi @Affie , thanks for all the effort and 1147 -- think we can close this issue now right?

[Affie]

I just want to do this before closing:

• Update RoME and confirm its working: Update factors to return residual RoME.jl#397
• Remove old parametric factor functors. (I don't think this should be blocking the tag as I still consider Parametric unstable and experimental) The treeSolve code is only halfway consolidated.

But, yes it can be closed, just want to keep track of whats outstanding.