Mixing factor and variable types (partial constraint)

[dehann]

This question came up: is it possible to only partially constrain a variable by using mix of factors. For example a landmark with orientation is stored as a Pose2, but now we only have bearing and/or range data to that landmark. How to programmatically resolve this?

[dehann]

It either already is, or should be, possible to just attach a "lesser" factor to a "bigger" variable. For example:

using RoME # or Caesar

fg = initfg()
addVariable!(fg, :x1, Pose2, labels=["POSE"])
addVariable!(fg,:l1, Pose2, labels=["LANDMARK"])

addFactor!(fg, [:x1], PriorPose2(MvNormal(zeros(3),1e-4*Matrix{Float64}(I,3,3))))
addFactor!(fg, [:x1; :l1], Pose2Point2BearingRange(Normal(),Normal()))

batchSolve!(fg)

The third variable (theta) of :l1 is not constrained and will therefore have random data in it. The expectation is that the code should still "solve" this graph. A quick test of the current code (RoME v0.3.0, IIF v0.5.0) produces these results:

julia> batchSolve!(fg)

Building Bayes net...
[ Info: 
[ Info: Eliminating 1
[ Info: ===============
[ Info: 
[ Info: 
[ Info: Eliminating 2
[ Info: ===============
[ Info: 
Bayes Net
Bayes Tree
Find potential functions for each clique
[ Info: Get potentials l1,x1,: 
[ Info: Ensure all nodes are initialized
[ Info: l1 is not initialized, and will do so now...
ERROR: During the resolution of the non-linear system, the evaluation of the following equation(s) resulted in a non-finite number: [2]

The current implementation requires more constraint to contain :l1 theta, however, there is no reason that it just remains a random value with warning.

Maybe the system should have a score per variable dimension so the user can probe the level constraint each dimension has...

[dehann]

This can be resolved by better using the existing partials infrastructure already in code. The challenge in this case is that we would need to do partial convolutions, where the current code supports partial products. More to follow...

[dehann]

existing partial poses include, https://github.com/JuliaRobotics/RoME.jl/blob/41fbf739de96014f4f2014d2d2d2cde0a8e3876e/src/factors/PartialPose3.jl

[dehann]

also see: https://github.com/JuliaRobotics/RoME.jl/blob/41fbf739de96014f4f2014d2d2d2cde0a8e3876e/test/testpartialpose3.jl