Issue with differential constraint from partial factors

[Affie]

Differential factors should be partial if source is only partial:

For example, if a child clique has a Pose2Point2Bearing at l1 | x1,x2 then the message can only be Pose2Pose2Bearing ie. partial on θ

Same for prior factors with partials.

EDIT: Needed for parametric tree solve.

[Affie]

This subfg from a range only example.

Results in these 2 factors after deconvolution.

x0 and x1 are connected by 2 range only factors and the differential factor cannot represent x1 fully.

[Affie]

For now useMsgLikelihoods=false should be used for better performance

useMsgLikelihoods=true

useMsgLikelihoods=false

[Affie]

Example plots above were adapted from the ranges examples from https://github.com/JuliaRobotics/RoME.jl/blob/master/examples/RangesExample.jl

EDIT: I just moved :l100 and :l3 for a clearer picture and used tighter covariances:

• GTp[:l100] = [-20.0;0]
• GTl[:l3] = [-30.0;20]

[dehann]

thanks, also linking some documentation on this that example for reference: https://juliarobotics.org/Caesar.jl/latest/examples/basic_slamedonut/

[dehann]

have not forgotten, just been a bit busy sorry. Will get to this soon!

[dehann]

Hi @Affie, unfortunately I'm not exctly sure how to precisely reconstruct the factor graphs for either of the two examples that you mentioned above. Could you perhaps update the guess below please, either the exact factor graphs via script or as simple saveDFG objects please (IIF v0.17 or up).

Needed for parametric tree solve.

#466 is required for the parametric case.

---

For the first example #1010 (comment) im assuming:

using RoME
fg = initfg()

addVariable!(fg, :x1, Pose2)
addVariable!(fg, :x2, Pose2)
addVariable!(fg, :l1, Point2)
addVariable!(fg, :l2, Point2)

addFactor!(fg, [:l1], PriorPoint2(..))
addFactor!(fg, [:l2], PriorPoint2(..))

addFactor!(fg, [:x1; :l1], Pose2Point2Bearing(..))
addFactor!(fg, [:x2; :l1], Pose2Point2Bearing(..))

addFactor!(fg, [:x1; :l2], Pose2Point2Bearing(..))
addFactor!(fg, [:x2; :l2], Pose2Point2Bearing(..))

---

for the second example did you just swap the Pose2Point2Bearing for Pose2Point2Range?

[dehann]

please modify the comments above and or update a FileDFG of the factor graphs which makes it a bit easier to compare etc.

[Affie]

The first one is just in general and not really a specific example.
I'll put up the code for the examples so you can modify them. I just fiddled with it a lot, so it's currently not the same as the plots.
:x0 from the above comment is actually :l100 in the example. I updated it to fix.

[Affie]

@dehann, this is basically just the example linked above with minor modifications.

using DistributedFactorGraphs
using IncrementalInference
using RoME
using RoMEPlotting
##

GTp = Dict{Symbol, Vector{Float64}}()
GTp[:x0] = [-20.0;0]
GTp[:x1] = [50.0;0]
GTp[:x2] = [100.0;0]
GTp[:x3] = [100.0;50.0]
GTp[:x4] = [100.0;100.0]

GTl = Dict{Symbol, Vector{Float64}}()
GTl[:l1] = [10.0;30]
GTl[:l2] = [30.0;-30]

usel3 = true
# usel3 && (GTl[:l3] = [80.0;40])
usel3 && (GTl[:l3] = [-30.0;20])

# σ=0.001
σ=0.5

# create the factor graph object
fg = initfg()

#set this to compare
# fg.solverParams.useMsgLikelihoods=false
# fg.solverParams.useMsgLikelihoods=true
#aslo try with graphinit by commenting the next 2 lines
fg.solverParams.graphinit=false
fg.solverParams.treeinit=true
fg.solverParams.N=300

addVariable!(fg, :x0, Point2)
addVariable!(fg, :l1, Point2)
addVariable!(fg, :l2, Point2)
usel3 && addVariable!(fg, :l3, Point2)

# lpσ=0.01
lpσ=1.0
addFactor!(fg, [:l1;], PriorPoint2(MvNormal(GTl[:l1], [lpσ, lpσ] )))
addFactor!(fg, [:l2;], PriorPoint2(MvNormal(GTl[:l2], [lpσ, lpσ] )))
# for a prior on l3
# addFactor!(fg, [:l3;], PriorPoint2(MvNormal(GTl[:l3], [lpσ, lpσ] )))

# first range measurement
rhoZ1 = norm(GTl[:l1]-GTp[:x0])
ppr = Point2Point2Range( Normal(rhoZ1, σ) )
addFactor!(fg, [:x0;:l1], ppr)

# second range measurement
rhoZ2 = norm(GTl[:l2]-GTp[:x0])
ppr = Point2Point2Range( Normal(rhoZ2, σ) )
addFactor!(fg, [:x0; :l2], ppr)

# third range measurement
if usel3
    rhoZ3 = norm(GTl[:l3]-GTp[:x0])
    ppr = Point2Point2Range( Normal(rhoZ3, σ) )
    addFactor!(fg, [:x0; :l3], ppr)
end

function vehicle_drives_to!(fgl::G, pos_sym::Symbol, GTp::Dict, GTl::Dict; measurelimit::R=250.0) where {G <: AbstractDFG, R <: Real}
  currvar = ls(fgl)
  prev_i = Base.parse(Int,string(pos_sym)[2:end]) - 1 
  prev_sym = Symbol("x$prev_i")
  if !(pos_sym in currvar)
    println("Adding variable vertex $pos_sym, not yet in fgl::AbstractDFG.")
    addVariable!(fgl, pos_sym, Point2)
    @show rho = norm(GTp[prev_sym] - GTp[pos_sym])
    ppr = Point2Point2Range( Normal(rho, σ) )
    addFactor!(fgl, [prev_sym;pos_sym], ppr)
  else
    @warn "Variable node $pos_sym already in the factor graph."
  end
  beacons = keys(GTl)
  for ll in beacons
    rho = norm(GTl[ll] - GTp[pos_sym])
    @show ll,rho
    # Check for feasible measurements:  vehicle within 150 units from the beacons/landmarks
    if rho < measurelimit
      ppr = Point2Point2Range( Normal(rho, σ) )
      if !(ll in currvar)
        println("Adding variable vertex $ll, not yet in fgl::AbstractDFG.")
        addVariable!(fgl, ll, Point2)
      end
      addFactor!(fgl, [pos_sym;ll], ppr)
    end
  end
  nothing
end



#

vehicle_drives_to!(fg, :x1, GTp, GTl)

smtasks = Task[]
tree, smt, hists = solveTree!(fg; smtasks, recordcliqs=ls(fg));

pl = plotKDE(fg, [:x0, :x1, :l1, :l2, :l3], c=["green"; "blue"; "red"; "cyan"; "magenta"], levels=2, dims=[1;2])

[dehann]

Great thanks, apologies for the detour. Working with this now!

[dehann]

Update on the plan here is to do a major upgrade in one go. There will be less waste overall by first doing JuliaRobotics/ApproxManifoldProducts.jl#41 first and then filling in the fixes for this issue during that refactor. Plan is to do that as last project for 2020 -- it's a 6 birds with 1 stone kinda thing.

The overall fix is likely to introduce the TangentAtlasFactors as replacement for current direct assumption :DIFFERENTIAL_FACTORS used by CSM today.

[dehann]

Just an update, I'm clearing the way for fixes required here. IIF v0.19 resolves a series of issues with CCW and FMD, while simultaneously preparing to solve

• Integrate with Manifolds.jl RoME.jl#244
• Ball/Metric Tree using NearestNeighbors.jl / Manifolds.jl / Distances.jl / CoordinateTransformations.jl ApproxManifoldProducts.jl#41 and
• this issue as related steps.

PS also see #1048

EDIT, was more sensible to first do #467

[dehann]

Also note a partial fix for easy relative likelihoods that follow default factors, which will be part of IIF v0.19 too (precursor to TAF).

[dehann]

All fixes related to #467 are precursors for a general code refactor of internal numerical operations, which will ultimately lead to the required fix on this issue. Work towards closing this issue is ongoing. 467 turned out to be a good pivot point to more efficiently address issues listed in #1010 (comment)

[dehann]

xref -- just adding cross link reference here that was previously lost in a old conversation:

• how to do Gaussian belief propagation on tree #464