Closed lemauee closed 4 years ago
just checking it now -- think #345 should fix the issue
Hi, thanks for reporting, we are working to include the fixes with IncrementalInference.jl v0.17.0 and RoME.jl v0.10.0 which will be coming out in the next week or so.
Hi @lemauee , after adding PRs above as well as in IncrementalInference.jl, please see updated example and locally computed quick test image:
The second hex is a little offset, not immediately sure why but likely due to something silly with how im calculating the angles. At least the bug your reported is fixed. If you learn anything new perhaps report it here or a new issue, or perhaps even submit a PR if something stands out. Thanks for reporting. Note the new stable tag on RoME should be coming out in a couple of days which will include all the updates mentioned. Best, Dehann
Thanks for the quick fixing! I am pretty new to Julia and have installed packages using only ']add package XXX' without any other setup to this date, maybe I'll take a look into installing from the master branch directly and not the release that the package manager gives me by default. I will test the example when I get the updates and also try my own scripts again. I will look into submitting PRs, some other examples seem to use deprecated or nonexistent functions that i could fix locally (replacing batchSolve! and some plotting stuff mostly). For example drawPosesLandms and plotSLAM2D seem to output the same, but replacing drawPosesLandms with plotSLAM2D seemed right because drawPosesLandms is written in deprecated.jl. Best, Leo
Hi, you can use ]add RoME#master
and ]add IncrementalInference#master
, new versions should be tagged soon and you can go back to stable by just adding the package again, ]add RoME
.
The examples are a bit behind, the files in test are up to date. You are correct with replacing batchSolve! with solveTree! and drawPosesLandmarks with plotSLAM2D.
Hi, thanks for the quick help for switching to the master branch. Sadly ]add RoME#master
and ]add IncrementalInference#master
throw an unsatisfiable requirements error:
ERROR: Unsatisfiable requirements detected for package RoMEPlotting [238d586b]: RoMEPlotting [238d586b] log: ├─possible versions are: [0.1.0-0.1.8, 0.2.0-0.2.2, 0.3.0, 0.4.0-0.4.2] or uninstalled ├─restricted to versions * by an explicit requirement, leaving only versions [0.1.0-0.1.8, 0.2.0-0.2.2, 0.3.0, 0.4.0-0.4.2] └─restricted by compatibility requirements with RoME [91fb55c2] to versions: uninstalled — no versions left └─RoME [91fb55c2] log: ├─possible versions are: 0.10.0 or uninstalled └─RoME [91fb55c2] is fixed to version 0.10.0
ERROR: Unsatisfiable requirements detected for package IncrementalInference [904591bb]: IncrementalInference [904591bb] log: ├─possible versions are: 0.17.0 or uninstalled ├─IncrementalInference [904591bb] is fixed to version 0.17.0 └─found to have no compatible versions left with RoMEPlotting [238d586b] └─RoMEPlotting [238d586b] log: ├─possible versions are: [0.1.0-0.1.8, 0.2.0-0.2.2, 0.3.0, 0.4.0-0.4.2] or uninstalled └─restricted to versions * by an explicit requirement, leaving only versions [0.1.0-0.1.8, 0.2.0-0.2.2, 0.3.0, 0.4.0-0.4.2]
After also switching to the master branch for RoMEPlotting (RoMEPlotting#master
) and running add RoME#master
again I get a similar error concerning Caesar:
ERROR: Unsatisfiable requirements detected for package Caesar [62eebf14]: Caesar [62eebf14] log: ├─possible versions are: [0.2.0-0.2.5, 0.3.0-0.3.1, 0.4.0-0.4.4, 0.5.0-0.5.3, 0.6.0-0.6.2] or uninstalled ├─restricted to versions * by an explicit requirement, leaving only versions [0.2.0-0.2.5, 0.3.0-0.3.1, 0.4.0-0.4.4, 0.5.0-0.5.3, 0.6.0-0.6.2] ├─restricted by compatibility requirements with Rotations [6038ab10] to versions: [0.4.0-0.4.4, 0.5.0-0.5.3, 0.6.0-0.6.2] or uninstalled, leaving only versions: [0.4.0-0.4.4, 0.5.0-0.5.3, 0.6.0-0.6.2] │ └─Rotations [6038ab10] log: │ ├─possible versions are: [0.7.2, 0.8.0, 0.9.0-0.9.2, 0.10.0, 0.11.0-0.11.1, 0.12.0-0.12.1, 0.13.0, 1.0.0-1.0.1] or uninstalled │ └─restricted to versions [0.12.1-0.13, 1] by RoME [91fb55c2], leaving only versions [0.12.1, 0.13.0, 1.0.0-1.0.1] │ └─RoME [91fb55c2] log: │ ├─possible versions are: 0.10.0 or uninstalled │ ├─restricted to versions 0.8-0.10 by RoMEPlotting [238d586b], leaving only versions 0.10.0 │ │ └─RoMEPlotting [238d586b] log: │ │ ├─possible versions are: 0.4.2 or uninstalled │ │ └─RoMEPlotting [238d586b] is fixed to version 0.4.2 │ └─RoME [91fb55c2] is fixed to version 0.10.0 └─restricted by compatibility requirements with RoME [91fb55c2] to versions: uninstalled — no versions left └─RoME [91fb55c2] log: see above
Also switching this to the master branch (add Caesar#master
) didn't help either, i suspect the RoME master is too new for the Caesar master?:
ERROR: Unsatisfiable requirements detected for package RoME [91fb55c2]: RoME [91fb55c2] log: ├─possible versions are: 0.10.0 or uninstalled ├─restricted to versions 0.7.1-0.9 by Caesar [62eebf14] — no versions left │ └─Caesar [62eebf14] log: │ ├─possible versions are: 0.6.2 or uninstalled │ └─Caesar [62eebf14] is fixed to version 0.6.2 └─RoME [91fb55c2] is fixed to version 0.10.0
Finally, IncrementalInference seems to be in a similar conflict with Caesar when running add IncrementalInference#master
now
ERROR: Unsatisfiable requirements detected for package IncrementalInference [904591bb]: IncrementalInference [904591bb] log: ├─possible versions are: 0.17.0 or uninstalled ├─restricted to versions 0.14.1-0.16 by Caesar [62eebf14] — no versions left │ └─Caesar [62eebf14] log: │ ├─possible versions are: 0.6.2 or uninstalled │ └─Caesar [62eebf14] is fixed to version 0.6.2 └─IncrementalInference [904591bb] is fixed to version 0.17.0
Sorry for posting such a long list of errors here :)
Yes you are correct, Caesar is lagging behind a bit. Caesar master should be updated within a few days. In the meantime, you can try without Caesar by just removing it from the environment. ]rm Caesar
We will tag IIF v0.17 within a few days followed by RoME v0.10 and RoMEPlotting and can then have a look at Caesar.
hope to tag stable versions of 4 interconnected packages in the next 24 hours and this bug should be fixed in them. apologies for the funky versions, you caught us right at the switchover of several internal breaking changes, so it's much easier for us to just do the bug fix on this issue along with the new system. thanks for posting
oh and as Affie pointed out, Pkg is very good. You'll quickly get the hang of it. in this case yes Caesar is one of the interdependencies. also see virtual environments, it's simple and reliable for controlling versions of packages: https://julialang.github.io/Pkg.jl/v1.0/index.html
Thanks for the quick help again, switching versions now works like charm, i can reproduce a similar output: But strangely the offset of the second hex is quite different from your solution. Is this due to the sampling based inner workings of mmiSAM that are bound to produce a different result every time or are you suspecting other causes?
Sadly, my own examples still do not work as expected. The one that worked when using the Pose2Point2BearingRange factor and produced clearly wrong results when using Pose2Point2Bearing and Pose2Point2Range now produces an error when solving:
LoadError: TaskFailedException:
BoundsError: attempt to access 2×1 Array{Float64,2} at index [1:2, 83]
Stacktrace:
[1] throw_boundserror(::Array{Float64,2}, ::Tuple{Base.Slice{Base.OneTo{Int64}},Int64}) at ./abstractarray.jl:541
[2] checkbounds at ./abstractarray.jl:506 [inlined]
[3] _getindex at ./multidimensional.jl:742 [inlined]
[4] getindex(::Array{Float64,2}, ::Function, ::Int64) at ./abstractarray.jl:1060
[5] solveFactorMeasurements(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::Symbol, ::Symbol) at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/DeconvUtils.jl:64
[6] solveFactorMeasurements at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/DeconvUtils.jl:31 [inlined]
[7] addLikelihoodsDifferential!(::LikelihoodMessage{IncrementalInference.NonparametricMessage}, ::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::LightDFG{SolverParams,DFGVariable,DFGFactor}) at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/TreeMessageUtils.jl:183
[8] addLikelihoodsDifferential! at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/TreeMessageUtils.jl:158 [inlined]
[9] addLikelihoodsDifferential! at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/TreeMessageUtils.jl:195 [inlined]
[10] addMsgFactors!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::LikelihoodMessage{IncrementalInference.NonparametricMessage}, ::Type{IncrementalInference.UpwardPass}; tags::Array{Symbol,1}) at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/TreeMessageUtils.jl:306
[11] addMsgFactors!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::Dict{Int64,LikelihoodMessage}, ::Type{IncrementalInference.UpwardPass}; tags::Array{Symbol,1}) at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/TreeMessageUtils.jl:335
[12] addMsgFactors! at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/TreeMessageUtils.jl:332 [inlined]
[13] preUpSolve_StateMachine(::CliqStateMachineContainer{BayesTreeNodeData,LightDFG{SolverParams,DFGVariable,DFGFactor},LightDFG{SolverParams,DFGVariable,DFGFactor},BayesTree}) at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/CliqueStateMachine.jl:212
[14] (::StateMachine{CliqStateMachineContainer})(::CliqStateMachineContainer{BayesTreeNodeData,LightDFG{SolverParams,DFGVariable,DFGFactor},LightDFG{SolverParams,DFGVariable,DFGFactor},BayesTree}, ::Nothing; pollinterval::Float64, breakafter::Function, verbose::Bool, verbosefid::Base.TTY, verboseXtra::IncrementalInference.CliqStatus, iterlimit::Int64, injectDelayBefore::Nothing, recordhistory::Bool, housekeeping_cb::IncrementalInference.var"#391#393"{IncrementalInference.TreeClique}) at /home/leopold/.julia/packages/FunctionalStateMachine/2JZFG/src/StateMachine.jl:82
[15] initStartCliqStateMachine!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::BayesTree, ::IncrementalInference.TreeClique, ::Int64; oldcliqdata::BayesTreeNodeData, verbose::Bool, drawtree::Bool, show::Bool, incremental::Bool, limititers::Int64, upsolve::Bool, downsolve::Bool, recordhistory::Bool, delay::Bool, logger::Base.CoreLogging.SimpleLogger, solve_progressbar::ProgressMeter.ProgressUnknown, algorithm::Symbol) at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/CliqueStateMachine.jl:64
[16] tryCliqStateMachineSolve!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::BayesTree, ::Int64; oldtree::BayesTree, verbose::Bool, drawtree::Bool, limititers::Int64, downsolve::Bool, incremental::Bool, delaycliqs::Array{Symbol,1}, recordcliqs::Array{Symbol,1}, solve_progressbar::ProgressMeter.ProgressUnknown, algorithm::Symbol) at /home/leopold/.julia/packages/IncrementalInference/m6lSZ/src/SolverAPI.jl:101
[17] (::IncrementalInference.var"#446#448"{BayesTree,Bool,Bool,Int64,Bool,Bool,Array{Symbol,1},Array{Symbol,1},Symbol,LightDFG{SolverParams,DFGVariable,DFGFactor},BayesTree,ProgressMeter.ProgressUnknown,Int64})() at ./task.jl:356
in expression starting at /home/leopold/workspace/Masterarbeit/svn/julia/mmiSAM/examples/example2D.jl:88
sync_end(::Channel{Any}) at task.jl:314
macro expansion at task.jl:333 [inlined]
taskSolveTree!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::BayesTree; oldtree::BayesTree, drawtree::Bool, verbose::Bool, limititers::Int64, downsolve::Bool, incremental::Bool, multithread::Bool, skipcliqids::Array{Symbol,1}, recordcliqs::Array{Symbol,1}, delaycliqs::Array{Symbol,1}, smtasks::Array{Task,1}, algorithm::Symbol) at SolverAPI.jl:46
(::IncrementalInference.var"#taskSolveTree!##kw")(::NamedTuple{(:algorithm, :multithread, :smtasks, :oldtree, :verbose, :drawtree, :recordcliqs, :limititers, :downsolve, :incremental, :skipcliqids, :delaycliqs),Tuple{Symbol,Bool,Array{Task,1},BayesTree,Bool,Bool,Array{Symbol,1},Int64,Bool,Bool,Array{Symbol,1},Array{Symbol,1}}}, ::typeof(IncrementalInference.taskSolveTree!), ::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::BayesTree) at SolverAPI.jl:33
solveTree!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::BayesTree; timeout::Nothing, storeOld::Bool, verbose::Bool, verbosefid::Base.TTY, delaycliqs::Array{Symbol,1}, recordcliqs::Array{Symbol,1}, limititercliqs::Array{Pair{Symbol,Int64},1}, injectDelayBefore::Nothing, skipcliqids::Array{Symbol,1}, maxparallel::Int64, variableOrder::Nothing, variableConstraints::Array{Symbol,1}, smtasks::Array{Task,1}, dotreedraw::Array{Int64,1}, runtaskmonitor::Bool, algorithm::Symbol, multithread::Bool) at SolverAPI.jl:342
(::IncrementalInference.var"#solveTree!##kw")(::NamedTuple{(:maxparallel,),Tuple{Int64}}, ::typeof(solveTree!), ::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::BayesTree) at SolverAPI.jl:268
(::IncrementalInference.var"#solveTree!##kw")(::NamedTuple{(:maxparallel,),Tuple{Int64}}, ::typeof(solveTree!), ::LightDFG{SolverParams,DFGVariable,DFGFactor}) at SolverAPI.jl:268
macro expansion at example2D.jl:120 [inlined]
top-level scope at timing.jl:233 [inlined]
top-level scope at example2D.jl:0
include_string(::Function, ::Module, ::String, ::String) at loading.jl:1088
Here's the FG producing this error when solving: fg-before-solve.tar.gz
Sadly, with the master-branch version of RoME, also the one using Pose2Point2BearingRange does not work anymore producing a similar error. Here's this file: fg-before-solve.tar.gz
What I noticed is that the plots of the initializations of those two (theoreticallly similar) graphs already look very different: Pose2Point2BearingRange: Pose2Point2Bearing Point2Point2Range seperately: So unfortunately the bugfix did not really solve the problem for me now.
I also came up with another toy example for bearing only SLAM that is running but does not produce the expected results. This might be a good starting point for further inspection:
# infer 5 poses and 3 LM by bearing measurements to LM and Odometry
## Load packages & add more Julia processes
using Distributed, Dates, RoME
nCores = 4;
if nCores > nprocs()
workers = addprocs(nCores-nprocs())
end
@everywhere using RoME
## generate GT
# robot positon
GTp = Dict{Symbol, Vector{Float64}}()
GTp[:x1] = [0.0;0.0;pi/4]
GTp[:x2] = [10.0;10.0;pi/4]
GTp[:x3] = [20.0;20.0;pi/4]
GTp[:x4] = [30.0;30.0;pi/4]
GTp[:x5] = [40.0;40.0;pi/4]
# landmark positon
GTl = Dict{Symbol, Vector{Float64}}()
GTl[:l1] = [50.0;0.0]
GTl[:l2] = [50.0;50.0]
GTl[:l3] = [0.0;50.0]
# odometry
GTo = Dict{Tuple{Symbol,Symbol}, Vector{Float64}}()
for kPose in 1:(length(GTp)-1)
p1s = Symbol("x", kPose)
p2s = Symbol("x", kPose+1)
ϕ1 = GTp[p1s][3]
x1 = GTp[p1s][1]
y1 = GTp[p1s][2]
ϕ2 = GTp[p2s][3]
x2 = GTp[p2s][1]
y2 = GTp[p2s][2]
T = [cos(ϕ1) -sin(ϕ1) x1; sin(ϕ1) cos(ϕ1) y1; 0 0 1]
p2wh = [x2; y2; 1];
p2bh = inv(T)*p2wh;
δϕ = ϕ2 - ϕ1;
tf = [p2bh[1]; p2bh[2]; δϕ];
GTo[(p1s,p2s)] = tf;
end
# bearings to LM
GTb = Dict{Tuple{Symbol,Symbol}, Float64}()
for pose in GTp
for lm in GTl
v = lm[2][1:2] - pose[2][1:2]
GTb[(pose[1],lm[1])] = wrapRad(atan(v[2],v[1])-pose[2][3])
end
end
## build FG
data_logpath = "bearingOnlySLAM2D"
fg = LightDFG{SolverParams}(solverParams=SolverParams(logpath=data_logpath))
# add vars
for pose in GTp
addVariable!(fg, pose[1], Pose2)
end
for lm in GTl
addVariable!(fg, lm[1], Point2)
end
# add factors
# prior on first pose
prior = MvNormal(GTp[:x1], Matrix(Diagonal([0.1;0.1;0.01].^2)))
addFactor!(fg, [:x1], PriorPoint2(prior))
# odometry
σp = 1.0; # TODO: maybe change
σφ = 0.1;
graphinit = false;
for odo in GTo
println(odo[2])
pp = Pose2Pose2(MvNormal(odo[2], Matrix(Diagonal([σp;σp;σφ].^2))))
addFactor!(fg, [odo[1][1]; odo[1][2]], pp, graphinit = graphinit)
end
# bearings
σ = 0.03;
graphinit = false;
for pose in GTp
for lm in GTl
ppb = Pose2Point2Bearing( Normal(GTb[pose[1],lm[1]], σ) )
addFactor!(fg, [pose[1]; lm[1]], ppb, graphinit = graphinit)
end
end
## solve FG
tree, smt, hist = solveTree!(fg, maxparallel=1000)`
Feel free to alter the example, as a MATLAB-native I'm clearly not using all of Julias capabilities to write this down nicer ;)
Sorry for the long text again. Are there any other (maybe also larger) examples using the Pose2Point2Bearing factor?
Best, Leo
Hi Leo, Thanks for the examples, we will have a look.
I also had strange behaviour with Pose2Point2Bearing
, it seemed to be influenced a lot by initialization.
FIY, I edited your post to make use of block quotes for the code to make it more readable. You can do:
#julia code block here
The error was triggered from a new feature that is on by default in IncrementalInference v0.17
It looks to run though with it turned off:
getSolverParams(fg).useMsgLikelihoods = false
I tested so long with the bearing range example:
@dehann , If I recordcliqs
and rerun the failed step it works. (it failed at the adding messages step addMsgFactors!(csmc.cliqSubFg, getMessageBuffer(csmc.cliq).upRx, UpwardPass)
)
EDIT: it is an intermittent error on addLikelihoodsDifferential!
julia> addLikelihoodsDifferential!(csmc19.cliqSubFg, beliefMsg)
ERROR: BoundsError: attempt to access 2×1 Array{Float64,2} at index [1:2, 61]
Stacktrace:
[1] throw_boundserror(::Array{Float64,2}, ::Tuple{Base.Slice{Base.OneTo{Int64}},Int64}) at ./abstractarray.jl:541
[2] checkbounds at ./abstractarray.jl:506 [inlined]
[3] _getindex at ./multidimensional.jl:742 [inlined]
[4] getindex(::Array{Float64,2}, ::Function, ::Int64) at ./abstractarray.jl:1060
[5] solveFactorMeasurements(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::Symbol, ::Symbol) at /home/johan/.julia/dev/IncrementalInference/src/DeconvUtils.jl:64
[6] solveFactorMeasurements at /home/johan/.julia/dev/IncrementalInference/src/DeconvUtils.jl:31 [inlined]
[7] addLikelihoodsDifferential!(::LikelihoodMessage{IncrementalInference.NonparametricMessage}, ::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::LightDFG{SolverParams,DFGVariable,DFGFactor}) at /home/johan/.julia/dev/IncrementalInference/src/TreeMessageUtils.jl:164
[8] addLikelihoodsDifferential! at /home/johan/.julia/dev/IncrementalInference/src/TreeMessageUtils.jl:139 [inlined]
[9] addLikelihoodsDifferential!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::LikelihoodMessage{IncrementalInference.NonparametricMessage}) at /home/johan/.julia/dev/IncrementalInference/src/TreeMessageUtils.jl:176
[10] top-level scope at REPL[208]:1
This is what I get for Pose2Point2Bearing
also with getSolverParams(fg).useMsgLikelihoods = false
Thanks for quick fix, i am now getting similar results for both Pose2Point2BearingRange
as well as Pose2Point2Bearing
and Pose2Point2Range
seperately
What still looks very different is the initialization:
For Pose2Point2BearingRange
:
For Pose2Point2Bearing
and Pose2Point2Range
:
I'll look what variables' KDE has its particles spread so far later today, it should be one of the landmarks.
Have you gotten any insights on the toy example yet?
Best, Leo
The initialization makes a big difference. Work on treeinit
and addLikelihoodsDifferential!
will improve this a lot, but I don't think Pose2Point2Range
works that well with addLikelihoodsDifferential!
yet.
In your toy example initialization is done just before solving the graph. You can get a better result by calling ensureAllInitialized!(fg)
just after adding the odo factors.
You can also limit the plot window to get a better idea of the results, eg. plotSLAM2D(fg, xmin=-10, xmax=70, ymin=-10, ymax=70)
The toy example works as expected if I take :l2
out of the game. It being in line with the poses causes (something like) a singularity for triangulation that breaks successful inference I'd say. This and the tip of initializing using odometry only gives me good results:
OT: Understanding the inner workings of Incremental Inference and RoME to track down problems like this myself (and not just using solveTree! blindly ;)) is another goal of mine. Are there any important resources besides the Caesar docs, the 'A Nonparametric Belief Solution to the Bayes Tree' Paper from 2016 and @dehann 's Thesis 'Multi-modal and Inertial Sensor Solutions for Navigation-type Factor Graphs' ?
Best, Leo
Hi Leo,
Yes the thesis is perhaps best resource at this point. It's getting a bit outdated and there is a bunch of new stuff we are working to publish.
Also see resources here: https://juliarobotics.org/Caesar.jl/latest/refs/literature/
FYI, IIF v0.17.0, RoME v0.10.0, RoMEPlotting v0.4.3, Caesar v0.6.3 are now tagged as stable and includes the fixes from this issue, thanks! You should be able to go back to regular stable versions whenever convenient. Note that by staying on #master you might find unexpected breaking changes as work on these repos continue.
julia> ]
pkg> add IncrementalInference RoME Caesar RoMEPlotting
That will point Pkg back to regular stable package versions. You can double check what's going on with:
pkg> status
I've been exploring the factor types for 2D SLAM in the past days and cannot get get a working example using the Pose2Point2Bearing factor. The Solver seems to obtain a wrong solution but is very confident about it. Even the script from the examples folder "Hexagonal2D_BearingOnly_SLAM.jl" has this behavior (see img attached). Also building a factor graph (using the same measurements) with Pose2Point2BearingRange factors works, but adding measurements separately as Pose2Point2Bearing and Point2Point2Range Measurements seems to break something.