API extension for constants & fields

[pvazteixeira]

(follow-up on blue sky discussion with @dehann)

Consider the application of terrain-based localization, where elevation measurements are used in conjunction with a map to estimate position. What should the syntax look like in this case?

One way to do this would be to evaluate the measurement likelihood and add it as a factor, e.g.:

pxz = measurementLikelihood(y, terrain)                # y is the elevation measurement
likelihoodSampler = AliasingScalarSampler(x, pxz)      # x is the discretized domain
addVariable!(fg, :x6, ContinuousEuclid{1})
addFactor!(fg, :x6, Prior(likelihoodSampler))

In this approach, the map is not captured explicitly through the factor graph, which may prove limiting when implementing new measurement models . One alternative could be to first add the map as a constant (i.e. a fixed/static) parameter:

addConstant!(fg, :em, ScalarField(terrain)) # terrain could be an array supporting interpolation

ScalarField would capture the elevation map and be used by the solver (by capturing the likelihoodSampler-based approach above inside the factor, for instance).

The map would then be referenced explicitly when adding a measurement:

addVariable!(fg, :x6, ContinuousEuclid{1})
addFactor!(fg, [:x6, :em], ScalarMeasurement(Normal(y,sigma_y) )

Constants stand in contrast to variables (i.e. addVariable), in that they are not parameters to be estimated.
If, however, this isn't the case, the map should be added as a variable, for example:

addVariable!(fg, :em, GriddedScalarField(terrain)) 
addVariable!(fg, :x6, ContinuousEuclid{1})
addFactor!(fg, [:x6, :em], ScalarMeasurement(Normal(y,sigma_y) )

In both cases (localization and mapping), the scalar fields are assumed to be static (no variation over time).

[dehann]

This is wonderful, thanks for the write-up! I'm confident we can make this work :-)

[dehann]

TODOs (from 18fe0ec#r47000979)

• basic 1D example script as factor graph (to mature into test later) -- PVT,
• plumbing for s.constants somehow gets us topoMap object -- DF,
• AliasingScalarSampler is not a 2D sampler, so expand to multidimensions.
• And figure out best way (later) to sample on various other manifolds -- may require some additional user plumbing for Constants

[dehann]

Random, http://juliaplots.org/MakieReferenceImages/gallery//streamplot_animation/index.html

[dehann]

Hi @pvazteixeira , did we perhaps have a reason not to call the API ‘addField!’ instead? I’m thinking of passing an argument to indicate either Constant, MalleablePrior, or Unknown — from there either Scalar or Vector.

[pvazteixeira]

Hmm, I don't think we did; we discussed addConstant vs addVariation and settled to the former - one of the reasons was that it was consistent with addVariable (1). My inclination is still

addVariable!(fg, dem, ScalarField) # for mapping
addConstant!(fg, dem, ScalarField) # for localization

The question I would ask is whether a scalar/vector field should be viewed the same as a Point, Pose, etc, or there is some aspect or use case that warrants a dedicated addField. What are your thoughts?

(1) since this wasn't captured in the original post (may be worth opening another item on it?), the consensus at the time on addVariation was that it would make more sense for use cases like parameter estimation - for example, add a parameter such as 'wheelbase' to a problem (with some initial value), treat it as fixed initially, and check the sensitivity of the current estimate against it.

[dehann]

addVariation1 ... for use cases like parameter estimation

100% agree, think we good there for now.

should be viewed the same as a Point, Pose, etc, or there is some aspect or use case that warrants a dedicated addField!.

My thinking there is reason to rather go addField!: Besides the implementation reasons, I'd say the main abstraction difference is whether we want to estimate an uncertain point (i.e. node in the graph); or whether this new "field thing" have an influence on many nodes in the graph. ( I actually said two things there, but hopefully clear).

From code and usage experience, it seems more natural for me to go, hey computer please note there is a general "field" around that could influences everybody, but I'll tell you which by adding a factor between the "field" and the "variable" of interest. That for me is the first tier.

The second tier is how much we know about the field. Either its a known (IIF.Constant, Malleable, Unknown). These cases correspond respectively to (localize, refine, map).

All this leads me to something like:

• TRN locailzation: addField!(fg, :topography, ScalarField{IIF.Constant,T}(...) )
• TRN mapping: addField!(fg, :topography, ScalarField{IIF.Unknown, T}(...) )
• Weak Prior Info addField!(fg, :echophase, VectorField{IIF.Malleable, N, T}(...))

In each of these cases a graph can be built out as:

# lets do this in factor graph form
fg = initfg()

# assume a known map
addField!(fg, :topography, ScalarField{IIF.Constant,T}(...) )

# add measurements
addVariable!(fg, :x0, Pose2)
addFactor!(fg, [:x0, :topography], ElevationSequence(relative_odo_seq, height_meas_seq, altimeter_meas_seq) )

# etc

I'm also wondering how this looks for gravity:

fg = initfg()

addField!( fg, :gravity, VectorField{IIF.Constant, 3, WGM2020}(data) )
# ...

and same for magnetics WMM2020, etc. Magnetics can easily be Malleable. on and on.

[pvazteixeira]

Can't say much about the implementation arguments, but the reasoning about fields makes sense. I do think we can cover all these use cases while keeping the same 'language'.

The second tier is how much we know about the field. Either its a known (IIF.Constant, Malleable, Unknown). These cases correspond respectively to (localize, refine, map).

Why not addConstant, addVariable+ addFactor(..., Prior(...)), and addVariable, for localize/refine/map, respectively? What I may be missing is what aspect(s) about mapping/refining/localizing against a field can't be captured by the existing API (assuming support for things like addConstant is present) and necessitate new terminology (e.g. Malleable). Conceptually at least, if you're going to estimate the field, should you not treat it as any other variable?

fg = initfg()

addConstant!(fg, :dem, ScalarField(...)) # use DEM data here, 
# alternatively:
# addVariable(fg, :dem, ScalarField)
# addFactor!(fg, :dem, ScalarFieldPrior) # use DEM data here

addVariable!(fg, :x0, Pose2)
addFactor!(fg, [:x0, :dem], ScalarMeasurement(baro_height-altitude, ...)) # single measurement

addVariable!(fg, :x1, Pose2)
addFactor!(fg, [:x1, :dem], ScalarSequence(odo_seq, scalar_seq)) # measurement sequence

Similarly, for magnetic and gravitational fields:

addConstant!(fg, :gravity, VectorField{3}(wgm_data)
addConstant!(fg, :mag, VectorField{3}(wmm_data)

# (...)

addFactor!(fg, [:x7, :mag], VectorMeasurement{3}(mag_meas))
# maybe even have :mag_cal as a `Variation` to handle calibration in post
addFactor!(fg, [:x8, :mag, :mag_cal], VectorMeasurement{3}(mag_meas)) 

addFactor!(fg, [:x9,:x10,:gravity], InertialMeasurement(acc, omega))

[dehann]

As I'm thinking through it, it's more of what should we do rather than can we do:

• I think this is a no go addPrior, because the bipartite language already has Factor as both likelihoods and priors. It would also cause way too much trouble in the implementation.
• The next breakdown for me is what is the difference between a Variable and a Field. Variable is familiar in bipartite language, Field is new.
  • Also, a Field can influence many Variables over long time and space horizons. Conventionally we only allow one Variable to influence another through the sparse connectivity.
  • I think Fields introduce a second tier (Constant/Fixed, Malleable, Unknown) which is poorly related to current bipartite Variable logic.
• Another thing that bothers me is addConstant!(fg, ... <<Unknown / Malleable>> ), which feels a bit contradictory.

ScalarField / VectorField

As identifier I think is good.

VectorMeasurement{N} / ScalarSequence

I'm still thinking about this one, but at first glance worried about how the mechanics are defined. Different field to variable relationships have unique mechanics, so "Measurement" here looks too generic (my first reaction).

The part that is tricky here is having to reverse identify what the user wants:
addFactor!(fg, [:x1, :dem], ScalarSequence(odo_seq, scalar_seq))

usually we use:
addFactor!(fg, [:x1, :x2], Pose2Pose(MvNormal(...)))
and don't need to reverse back to :dem is a ScalarField{T} and because it's T there is only one known method in the library for combining ScalarSequence with ScalarField{T} -- but what if the user wants to build out ScalarField{T2} and also use ScalarSequence? Looks like that might become hard.

Another thing to remember is during development we might build everything using addField! and then later just consolidate into addConstant!, addVariable!, addFactor! if it is easily understandable. However, the reverse might be much harder -- if we run into trouble and then need to undo tricks towards a unique addField! instead.

Regarding the implementation, the Nouns and actions relating to Variable and Factor are very specific. I'm having a hard time trying to imagine all the permutations in advance, so instinct tells me to rather implement cautiously on a "new" API and then consolidate once the big moving pieces are in place. Changing the API is relatively easy, compared to fiddling with the Abstractions which might later beach on a API problem.

[pvazteixeira]

I think this is a no go addPrior, because the bipartite language already has Factor as both likelihoods and priors. It would also cause way too much trouble in the implementation.

Absolutely - that was a mistake on my end, I should have written addFactor!(..., Prior(...))

The next breakdown for me is what is the difference between a Variable and a Field. Variable is familiar in bipartite language, Field is new.

• Also, a Field can influence many Variables over long time and space horizons. Conventionally we only allow one Variable to influence another through the sparse connectivity.
• I think Fields introduce a second tier (Constant/Fixed, Malleable, Unknown) which is poorly related to current bipartite Variable logic.

This is what I am trying to get at - there is no need for this second tier/additional language if you consider a field to be a variable type (just like a Pose3), which it may very well be as you're tying to estimate it. Yes, it will negatively impact sparsity, but isn't that true of any approach given the nature of the measurement? Not to go on a tangent, but isn't this the same problem as having, say, a Pose3 capture the transformation between sensor and platform?

Another thing that bothers me is addConstant!(fg, ... <<Unknown / Malleable>> ), which feels a bit contradictory.

Yes - and this is why I'm arguing to avoid the Unknown/Malleable terminology altogether - a field is either a constant or a variable and, in the latter case, there may or may not be prior information available. A question that may help inform this discussion is how to represent field variables. Is it always gridded data?

Another thing to remember is during development we might build everything using addField! and then later just consolidate into addConstant!, addVariable!, addFactor! if it is easily understandable. However, the reverse might be much harder -- if we run into trouble and then need to undo tricks towards a unique addField! instead.

This is reasonable - would you leave addField! as a deprecated method if you later went the field-as-variable route? I can't speak to what makes the most sense development-wise, as you're more familiar with the code than anyone else 😄

(edit: hit 'reply' too soon)

[dehann]

Hi @pvazteixeira ,

I got the boxy example to get a little further. There might be a odo relative correlator offset bug again:

Caesar.jl/examples/dev/scalar/boxy.jl

Lines 292 to 295 in 6edbdb4

	pts = approxConv(fg, :x0x4f1, :x4)
	# should peak at +5 to make the relative factor represent the change between x0 dn x4
	Gadfly.plot(x=pts[1,:], Geom.histogram)

The offset between x0a dn x4 is 5 units, and here is what we get out from the above histogram plot on :x0x4f1 northing:

I recall that changed back to 0 with odo included, but we still need to add that odo estimate to the correlator intensity shift before we build the Aliasing Sampler. This is because the factor mechanics:

Caesar.jl/examples/dev/scalar/boxy.jl

Line 247 in 6edbdb4

(s::CalcFactor{<:Point2Point2Northing})(z, x1, x2) = z .- (x2[1] - x1[1])

do not include the estimated odo offset. So we just need to account for the odo somewhere at the last stages (probably after the correlator intensity has been calculated) before the Aliasing Scalar Sampler is built.