diff --git a/Project.toml b/Project.toml index e3a9a4982..526701dd4 100644 --- a/Project.toml +++ b/Project.toml @@ -2,7 +2,7 @@ name = "Caesar" uuid = "62eebf14-49bc-5f46-9df9-f7b7ef379406" keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics", "ROS"] desc = "Non-Gaussian simultaneous localization and mapping" -version = "0.7.1" +version = "0.8.0" [deps] ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89" @@ -54,7 +54,7 @@ FFTW = "1" FileIO = "1" ImageCore = "0.7, 0.8, 0.9" ImageMagick = "0.7, 1.0, 1.1" -IncrementalInference = "0.16, 0.17, 0.18, 0.19" +IncrementalInference = "0.20.1" JLD2 = "0.1, 0.2, 0.3" JSON = "0.18, 0.19, 0.20, 0.21, 0.22, 0.23" JSON2 = "0.3, 0.4" @@ -64,7 +64,7 @@ Optim = "1" ProgressMeter = "0.9, 1" Reexport = "0.2, 1.0" Requires = "0.5, 0.6, 0.7, 0.8, 0.9, 0.10, 1" -RoME = "0.9, 0.10, 0.11" +RoME = "0.10, 0.11, 0.12" Rotations = "0.13, 1" TensorCast = "0.2, 0.3" TimeZones = "1.3.1, 1.4" diff --git a/docs/src/concepts/adding_variables_factors.md b/docs/src/concepts/adding_variables_factors.md index b968e7d3a..fc668ccb7 100644 --- a/docs/src/concepts/adding_variables_factors.md +++ b/docs/src/concepts/adding_variables_factors.md @@ -94,20 +94,20 @@ In a trivial example of Pose2: All factors inherit from one of the following types, depending on their function: * AbstractPrior: AbstractPrior are priors (unary factors) that provide an absolute constraint for a single variable. A simple example of this is an absolute GPS prior, or equivalently a (0, 0, 0) starting location in a [`Pose2`](@ref) scenario. * Requires: A getSample function -* AbstractRelativeFactorMinimize: AbstractRelativeFactorMinimize are relative factors that introduce an algebraic relationship between two or more variables. A simple example of this is an odometry factor between two pose variables, or a range factor indicating the range between a pose and another variable. +* AbstractRelativeMinimize: AbstractRelativeMinimize are relative factors that introduce an algebraic relationship between two or more variables. A simple example of this is an odometry factor between two pose variables, or a range factor indicating the range between a pose and another variable. * Requires: A getSample function and a residual function definition * The minimize suffix specifies that the residual function of this factor will be enforced by numerical minimization (find me the minimum of this function) -* AbstractRelativeFactor: AbstractRelativeFactor are relative factors that introduce algebraic relationships between two or more variables. They are the same as AbstractRelativeFactorMinimize, however they use root finding to find the zero crossings (rather than numerical minimization). +* AbstractRelativeRoots: AbstractRelativeRoots are relative factors that introduce algebraic relationships between two or more variables. They are the same as AbstractRelativeMinimize, however they use root finding to find the zero crossings (rather than numerical minimization). * Requires: A getSample function and a residual function definition How do you decide which to use? * If you are creating factors for world-frame information that will be tied to a single variable, inherit from AbstractPrior * GPS coordinates should be priors -* If you are creating factors for local-frame relationships between variables, inherit from AbstractRelativeFactorMinimize +* If you are creating factors for local-frame relationships between variables, inherit from AbstractRelativeMinimize * Odometry and bearing deltas should be introduced as pairwise factors and should be local frame -TBD: sUsers **should** start with AbstractRelativeFactorMinimize, discuss why and when they should promote their factors to AbstractRelativeFactor. +TBD: sUsers **should** start with AbstractRelativeMinimize, discuss why and when they should promote their factors to AbstractRelativeRoots. -> Note: AbstractRelativeFactorMinimize does not imply that the overall inference algorithm only minimizes an objective function. The Multi-model iSAM algorithm is built around fixed-point analysis. Minimization is used here to locally enforce the residual function. +> Note: AbstractRelativeMinimize does not imply that the overall inference algorithm only minimizes an objective function. The Multi-model iSAM algorithm is built around fixed-point analysis. Minimization is used here to locally enforce the residual function. What you need to build in the new factor: * A struct for the factor itself @@ -131,7 +131,7 @@ AbstractRelativeRoots ### Pose2Point2BearingRange Struct ```julia -mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeFactor +mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeRoots bearing::B range::R Pose2Point2BearingRange{B,R}() where {B,R} = new{B,R}() diff --git a/docs/src/concepts/available_varfacs.md b/docs/src/concepts/available_varfacs.md index cd0563570..41f07326d 100644 --- a/docs/src/concepts/available_varfacs.md +++ b/docs/src/concepts/available_varfacs.md @@ -46,11 +46,11 @@ You can check for the latest factor types by running the following in your termi ```julia using RoME, Caesar println("- Singletons (priors): ") -println.(sort(string.(subtypes(IncrementalInference.AbstractPrior)))); +println.(sort(string.(subtypes(IIF.AbstractPrior)))); println("- Pairwise (variable constraints): ") -println.(sort(string.(subtypes(IncrementalInference.AbstractRelativeFactor)))); +println.(sort(string.(subtypes(IIF.AbstractRelativeRoots)))); println("- Pairwise (variable minimization constraints): ") -println.(sort(string.(subtypes(IncrementalInference.AbstractRelativeFactorMinimize)))); +println.(sort(string.(subtypes(IIF.AbstractRelativeMinimize)))); ``` ### Priors (Absolute Data) diff --git a/docs/src/examples/basic_definingfactors.md b/docs/src/examples/basic_definingfactors.md index be1b66b7c..4a0d84e7b 100644 --- a/docs/src/examples/basic_definingfactors.md +++ b/docs/src/examples/basic_definingfactors.md @@ -21,7 +21,7 @@ Prior(t::T) where {T <: SamplableBelief} = Prior{T}(t) # sampling function getSample(s::Prior, N::Int=1) = (reshape(rand(s.z,N),1,:), ) -struct LinearOffset{T} <: IncrementalInference.AbstractRelativeFactor where T <: SamplableBelief +struct LinearOffset{T <: SamplableBelief} <: IncrementalInference.AbstractRelativeRoots z::T end getSample(s::LinearOffset, N::Int=1) = (reshape(rand(s.z,N),1,:), ) diff --git a/docs/src/examples/interm_dynpose.md b/docs/src/examples/interm_dynpose.md index 17882d2bc..ede4a9151 100644 --- a/docs/src/examples/interm_dynpose.md +++ b/docs/src/examples/interm_dynpose.md @@ -52,6 +52,9 @@ getSample(dp2v::DynPoint2VelocityPrior, N::Int=1) = (rand(dp2v.z,N), ) ### `DynPoint2DynPoint2` (preintegration) +!!! warning + `::IIF.FactorMetadata` is being refactored and improved. Some of the content below is out of date. See IIF #1025 for details. (1Q2021) + The basic idea is that change in position is composed of three components (originating from double integration of Newton's second law): ![deltapositionplus](https://user-images.githubusercontent.com/6412556/40951449-05bdfed8-6845-11e8-8c4f-31fd92523819.gif) diff --git a/docs/src/principles/bayestreePrinciples.md b/docs/src/principles/bayestreePrinciples.md index 3d5c0dde8..7d0df3d38 100644 --- a/docs/src/principles/bayestreePrinciples.md +++ b/docs/src/principles/bayestreePrinciples.md @@ -34,13 +34,13 @@ fg = generateCanonicalFG_Kaess() # ... ## build the tree -tree = resetBuildTree!(fg) +tree = buildTreeReset!(fg) ``` -The temporary values are `reset` from the distributed factor graph object `fg<:AbstractDFG` and a new tree is constructed. This `resetBuildTree!` call can be repeated as many times the user desires and results should be consistent for the same factor graph structure (regardless of numerical values contained within). +The temporary values are `reset` from the distributed factor graph object `fg<:AbstractDFG` and a new tree is constructed. This `buildTreeReset!` call can be repeated as many times the user desires and results should be consistent for the same factor graph structure (regardless of numerical values contained within). ```@docs -resetBuildTree! +buildTreeReset! ``` ## Variable Ordering @@ -60,11 +60,7 @@ vo = [:x1; :l3; :x2; ...] And then reset the factor graph and build a new tree ```julia -resetBuildTreeFromOrder!(fg, vo) -``` - -```@docs -resetBuildTreeFromOrder! +buildTreeReset!(fg, vo) ``` !!! note @@ -87,7 +83,7 @@ tree, smt, hist = solveTree!(fg) ### Get the Elimination Order Used -The solver internally uses [`resetBuildTree!`](@ref) which sometimes requires the user extract the variable elimination order after the fact. This can be done with: +The solver internally uses [`buildTreeReset!`](@ref) which sometimes requires the user extract the variable elimination order after the fact. This can be done with: ```@docs getEliminationOrder ``` diff --git a/examples/basic/quickBatchSolve.jl b/examples/basic/quickBatchSolve.jl index a9bee9624..5f7b74255 100644 --- a/examples/basic/quickBatchSolve.jl +++ b/examples/basic/quickBatchSolve.jl @@ -23,7 +23,9 @@ factrs = setdiff(lsf(fg), lsf(fg, r"drt")) ensureAllInitialized!(fg) getSolverParams(fg).drawtree = true -@time solveTree!(fg, maxparallel=1000); +getSolverParams(fg).maxincidence = 1000 + +@time solveTree!(fg); savepath = splitpath(ARGS[1]) savename = split(savepath[end], '.')[1] diff --git a/examples/marine/asv/kayaks/PointSource_script.jl b/examples/marine/asv/kayaks/PointSource_script.jl index 3d6f0deb0..a66827bbd 100644 --- a/examples/marine/asv/kayaks/PointSource_script.jl +++ b/examples/marine/asv/kayaks/PointSource_script.jl @@ -102,8 +102,9 @@ writeGraphPdf(fg,viewerapp="", engine="neato", filepath = "/tmp/test.pdf") # getSolverParams(fg).drawtree = true getSolverParams(fg).limititers=500 #getSolverParams(fg).showtree = true +getSolverParams(fg).maxincidence = 200 -tree, smt, hist = solveTree!(fg,maxparallel=200) +tree, smt, hist = solveTree!(fg) # Some Debuggging # @time L1 = approxConv(fg,:l1x1x2x3x4x5x6x7x8f1,:l1) diff --git a/examples/marine/asv/kayaks/dynsas.jl b/examples/marine/asv/kayaks/dynsas.jl index 39e21e7fa..9cc4d3923 100644 --- a/examples/marine/asv/kayaks/dynsas.jl +++ b/examples/marine/asv/kayaks/dynsas.jl @@ -52,6 +52,7 @@ cfgFile = joinpath(ENV["HOME"],"data","sas","SAS2D.yaml") cfgd=loadConfigFile(cfgFile) fg = initfg(); +getSolverParams(fg).maxincidence = 400 beacon = :l1 addVariable!(fg, beacon, Point2 ) tree = emptyBayesTree(); @@ -124,9 +125,9 @@ while pose_counter < totalposes-2 setValKDE!(fg,lstpose,XXkde); if sas_counter > 1 - tree, smt, hist = solveTree!(fg,tree, maxparallel=400) + tree, smt, hist = solveTree!(fg,tree) else - tree, smt, hist = solveTree!(fg, maxparallel=400) + tree, smt, hist = solveTree!(fg) end writeGraphPdf(fg,viewerapp="", engine="neato", filepath=scriptHeader*"fg.pdf") diff --git a/examples/marine/asv/kayaks/dynsas_ljscript.jl b/examples/marine/asv/kayaks/dynsas_ljscript.jl index 2459e1a5d..7d46135aa 100644 --- a/examples/marine/asv/kayaks/dynsas_ljscript.jl +++ b/examples/marine/asv/kayaks/dynsas_ljscript.jl @@ -46,9 +46,11 @@ function main(expID::String, datastart::Int, dataend::Int, fgap::Int, gps_gap::I cfgd=loadConfigFile(cfgFile) fg = initfg(); + getSolverParams(fg).maxincidence = 1000 + beacon = :l1 addVariable!(fg, beacon, Point2 ) - tree = emptyBayesTree(); + tree = BayesTree(); pose_counter = 1 sas_counter = 1 @@ -114,9 +116,9 @@ function main(expID::String, datastart::Int, dataend::Int, fgap::Int, gps_gap::I setValKDE!(fg,lstpose,XXkde); if sas_counter > 1 - tree, smt, hist = solveTree!(fg,tree, maxparallel=400) + tree, smt, hist = solveTree!(fg,tree) else - tree, smt, hist = solveTree!(fg, maxparallel=400) + tree, smt, hist = solveTree!(fg) end writeGraphPdf(fg,viewerapp="", engine="neato", filepath=scriptHeader*"fg.pdf") diff --git a/examples/marine/asv/kayaks/dynsas_timed.jl b/examples/marine/asv/kayaks/dynsas_timed.jl index ee71c5984..4bbda9d32 100644 --- a/examples/marine/asv/kayaks/dynsas_timed.jl +++ b/examples/marine/asv/kayaks/dynsas_timed.jl @@ -52,9 +52,11 @@ function main(expID::String, datastart::Int, dataend::Int, fgap::Int, gps_gap::I cfgd=loadConfigFile(cfgFile) fg = initfg(); + getSolverParams(fg).maxincidence = 400 + beacon = :l1 addVariable!(fg, beacon, Point2 ) - tree = emptyBayesTree(); + tree = BayesTree(); pose_counter = 1 sas_counter = 1 @@ -123,9 +125,9 @@ function main(expID::String, datastart::Int, dataend::Int, fgap::Int, gps_gap::I println("Solving at SAS-F: $(sas_counter), Pos: $(pose_counter) \n"); if sas_counter > 1 - @time tree, smt, hist = solveTree!(fg,tree, maxparallel=400) + @time tree, smt, hist = solveTree!(fg,tree) else - @time tree, smt, hist = solveTree!(fg, maxparallel=400) + @time tree, smt, hist = solveTree!(fg) end # writeGraphPdf(fg,viewerapp="", engine="neato", filepath=scriptHeader*"fg.pdf") diff --git a/examples/marine/asv/kayaks/rangeOnlySLAM.jl b/examples/marine/asv/kayaks/rangeOnlySLAM.jl index 5593f71a5..389998fb0 100644 --- a/examples/marine/asv/kayaks/rangeOnlySLAM.jl +++ b/examples/marine/asv/kayaks/rangeOnlySLAM.jl @@ -76,12 +76,12 @@ getSolverParams(fg).showtree = false getSolverParams(fg).async = true getSolverParams(fg).multiproc = true getSolverParams(fg).downsolve = true +getSolverParams(fg).maxincidence = 100 - -tree, smt, hist = solveTree!(fg, maxparallel=100) +tree, smt, hist = solveTree!(fg) drawTree(tree,filepath = "/tmp/test.pdf") # fg2 = deepcopy(fg) -# tree, smt, hist = solveTree!(fg,tree,maxparallel=100) +# tree, smt, hist = solveTree!(fg,tree) # plotSASDefault(fg,expID, posData,igt,datadir=allpaths[1],savedir=scriptHeader*"SASdefault.pdf") plotSASDefault(fg,expID, posData,igt,dposData, datadir=allpaths[1], savedir="/tmp/caesar/test.pdf") diff --git a/examples/marine/asv/kayaks/rangeOnlySLAM_dbg.jl b/examples/marine/asv/kayaks/rangeOnlySLAM_dbg.jl index 67bd720d6..48a5d753d 100644 --- a/examples/marine/asv/kayaks/rangeOnlySLAM_dbg.jl +++ b/examples/marine/asv/kayaks/rangeOnlySLAM_dbg.jl @@ -81,16 +81,17 @@ getSolverParams(fg).async = true getSolverParams(fg).multiproc = false getSolverParams(fg).upsolve = true getSolverParams(fg).downsolve = false +getSolverParams(fg).maxincidence = 100 # writeGraphPdf(fg, show=true) ensureAllInitialized!(fg) -tree, smt, hist = solveTree!(fg, maxparallel=100) +tree, smt, hist = solveTree!(fg0) drawTree(tree,filepath = "/tmp/caesar/bt.pdf", show=true, imgs=false) # fg2 = deepcopy(fg) -# tree, smt, hist = solveTree!(fg,tree,maxparallel=100) +# tree, smt, hist = solveTree!(fg,tree) # plotSASDefault(fg,expID, posData, igt, dposData, datadir=allpaths[1],savedir="/tmp/caesar/plotsas.pdf"); # # plotSASDefault(fg,expID, posData,igt,dposData, datadir=allpaths[1], savedir="/tmp/caesar/test.pdf") @@ -312,21 +313,22 @@ getSolverParams(fg).async = true getSolverParams(fg).multiproc = false getSolverParams(fg).upsolve = true getSolverParams(fg).downsolve = false +getSolverParams(fg).maxincidence = 100 # writeGraphPdf(fg, show=true) # ensureAllInitialized!(fg) -tree, smt, hist = solveTree!(fg, maxparallel=100) +tree, smt, hist = solveTree!(fg) -plotKDE(fg, reverse(sortVarNested(ls(fg, r"x"))), levels=1) +plotKDE(fg, reverse(sortDFG(ls(fg, r"x"))), levels=1) getSolverParams(fg).upsolve = false getSolverParams(fg).downsolve = true -tree, smt, hist = solveTree!(fg, maxparallel=100) +tree, smt, hist = solveTree!(fg) diff --git a/examples/marine/asv/kayaks/rangeOnlySLAM_ljscript.jl b/examples/marine/asv/kayaks/rangeOnlySLAM_ljscript.jl index 4b32ffbd3..ad820465e 100644 --- a/examples/marine/asv/kayaks/rangeOnlySLAM_ljscript.jl +++ b/examples/marine/asv/kayaks/rangeOnlySLAM_ljscript.jl @@ -68,10 +68,11 @@ function main(expID::String, rangegap::Int, wstart::Int, wend::Int, trialID::Int getSolverParams(fg).drawtree = false getSolverParams(fg).showtree = false getSolverParams(fg).limititers=500 + getSolverParams(fg).maxincidence = 400 - tree, smt, hist = solveTree!(fg, maxparallel=400) - tree, smt, hist = solveTree!(fg, maxparallel=400) + tree, smt, hist = solveTree!(fg) + tree, smt, hist = solveTree!(fg) drawTree(tree, filepath=scriptHeader*"bt.pdf") plotSASDefault(fg,expID, posData,igt,dposData,datadir=allpaths[1],savedir=scriptHeader*"SASdefault.pdf") diff --git a/examples/marine/asv/kayaks/rangeOnlySLAM_timed.jl b/examples/marine/asv/kayaks/rangeOnlySLAM_timed.jl index bcaf2db21..bc5b1e8c5 100644 --- a/examples/marine/asv/kayaks/rangeOnlySLAM_timed.jl +++ b/examples/marine/asv/kayaks/rangeOnlySLAM_timed.jl @@ -70,12 +70,13 @@ function main(expID::String, rangegap::Int, wstart::Int, wend::Int, trialID::Int getSolverParams(fg).drawtree = false getSolverParams(fg).showtree = false getSolverParams(fg).limititers=500 + getSolverParams(fg).maxincidence = 400 - tree, smt, hist = solveTree!(fg, maxparallel=400) + tree, smt, hist = solveTree!(fg) println("Ended Script with $(rangecounter) range factors \n") - # tree, smt, hist = solveTree!(fg, maxparallel=400) + # tree, smt, hist = solveTree!(fg) # drawTree(tree, filepath=scriptHeader*"bt.pdf") # plotSASDefault(fg,expID, posData,igt,dposData,datadir=allpaths[1],savedir=scriptHeader*"SASdefault.pdf") diff --git a/examples/marine/asv/kayaks/testRangeOnlyOdo.jl b/examples/marine/asv/kayaks/testRangeOnlyOdo.jl index fee2a6059..559cdcc67 100644 --- a/examples/marine/asv/kayaks/testRangeOnlyOdo.jl +++ b/examples/marine/asv/kayaks/testRangeOnlyOdo.jl @@ -70,10 +70,11 @@ end getSolverParams(fg).drawtree = true getSolverParams(fg).showtree = false getSolverParams(fg).dbg = true +getSolverParams(fg).maxincidence = 200 ensureAllInitialized!(fg) -tree, smt, hist = solveTree!(fg, maxparallel=200, recordcliqs=[:x19;:x169;:x166;:x134;:x171]) +tree, smt, hist = solveTree!(fg, recordcliqs=[:x19;:x169;:x166;:x134;:x171]) fetchCliqTaskHistoryAll!(smt, hist) @@ -137,8 +138,6 @@ Gadfly.plot(x=out,Geom.histogram,Coord.cartesian(xmin=0, xmax=100, ymin=0, ymax= using RoMEPlotting -# tree = wipeBuildNewTree!(fg, maxparallel=200) -# spyCliqMat(tree, :x169) drawTree(tree, show=true) diff --git a/examples/marine/auv/Sandshark/LCM_SLAM_client.jl b/examples/marine/auv/Sandshark/LCM_SLAM_client.jl index cce86080b..feae2b89a 100644 --- a/examples/marine/auv/Sandshark/LCM_SLAM_client.jl +++ b/examples/marine/auv/Sandshark/LCM_SLAM_client.jl @@ -256,7 +256,8 @@ end dontMarginalizeVariablesAll!(fg) setSolvable!(getVariable(fg, :drt_ref), 0) -tree, smt, hist = solveTree!(fg, maxparallel=1000) +getSolverParams(fg).maxincidence = 1000 +tree, smt, hist = solveTree!(fg) saveDFG(fg, joinpath(getLogPath(fg),"fg_batchsolve") ) diff --git a/examples/marine/auv/Sandshark/SandsharkUtils.jl b/examples/marine/auv/Sandshark/SandsharkUtils.jl index 19f37c661..940e164f4 100644 --- a/examples/marine/auv/Sandshark/SandsharkUtils.jl +++ b/examples/marine/auv/Sandshark/SandsharkUtils.jl @@ -274,6 +274,7 @@ end # getSolverParams(dfg).qfl = dashboard[:SOLVESTRIDE] # getSolverParams(dfg).isfixedlag = true # getSolverParams(dfg).limitfixeddown = limitfixeddown +# getSolverParams(dfg).maxincidence = 1000 # # # allow async process # # getSolverParams(dfg).async = true @@ -337,7 +338,7 @@ end # dt_save1 = (time_ns()-t0)/1e9 # # constrain solve with the latest pose at the top # # @show latestPose = intersect(getLastPoses(dfg, filterLabel=r"x\d", number=12), ls(dfg, r"x\d", solvable=1))[end] -# tree, smt, hist = solveTree!(dfg, tree, maxparallel=1000) # , variableConstraints=[latestPose;] +# tree, smt, hist = solveTree!(dfg, tree) # , variableConstraints=[latestPose;] # dt_solve = (time_ns()-t0)/1e9 # !dbg ? nothing : saveDFG(dfg, joinpath(getLogPath(dfg), "fg_after_$(lasp)")) # diff --git a/examples/marine/auv/Sandshark/Sandshark_script_pieces.jl b/examples/marine/auv/Sandshark/Sandshark_script_pieces.jl index d55bb852d..d70aeaf90 100644 --- a/examples/marine/auv/Sandshark/Sandshark_script_pieces.jl +++ b/examples/marine/auv/Sandshark/Sandshark_script_pieces.jl @@ -203,6 +203,8 @@ tree1 = wipeBuildNewTree!(fg1) # getSolverParams(fg1).isfixedlag = true # getSolverParams(fg1).qfl = 20 +getSolverParams(fg1).maxincidence = 200 + # first solve and initialization getSolverParams(fg1).drawtree = true # getSolverParams(fg).showtree = false @@ -226,7 +228,7 @@ for STEP in 0:10:50 global storeLast runEpochs!(fg1, epochs, STEP, index1, acousticRate=3) - poses = sortVarNested(ls(fg1, r"x")) + poses = sortDFG(ls(fg1, r"x")) if STEP-laglength >= 0 @show poses[1], poses[end], poses[end-laglength] @@ -239,7 +241,7 @@ for STEP in 0:10:50 drawGraph(fg1, filepath=joinpath(getSolverParams(fg1).logpath, "graphs", "fg_$(STEP)_.pdf")) saveDFG(fg1, joinpath(getSolverParams(fg1).logpath, "graphs", "fg_$(STEP)_before")) - @time tree1, smt, hist = solveTree!(fg1, tree1, maxparallel=200) + @time tree1, smt, hist = solveTree!(fg1, tree1) saveDFG(fg1, joinpath(getSolverParams(fg1).logpath, "graphs", "fg_$(STEP)_after")) pla = drawPosesLandmarksAndOdo(fg1, ppbrDict, navkeys, X, Y, lblkeys, lblX, lblY) diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index 821923670..7d7c77ef6 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -212,6 +212,7 @@ getSolverParams(fg).logpath = resultsdir getSolverParams(fg).multiproc=multiproc getSolverParams(fg).drawtree=drawtree getSolverParams(fg).showtree=false +getSolverParams(fg).maxincidence = 500 prev_psid = 0 # load from previous file @@ -266,7 +267,7 @@ for psid in (prev_psid+1):1:maxlen if psid % BB == 0 || psid == maxlen saveDFG(fg, resultsdir*"/racecar_fg_$(psym)_presolve") # , drawpdf=true, show=show, N=N, recursive=true - tree, smt, hist = solveTree!(fg, tree, maxparallel=500) + tree, smt, hist = solveTree!(fg, tree) end # T1 = remotecall(saveDFG, WP, fg, resultsdir*"/racecar_fg_$(psym)") @@ -292,7 +293,7 @@ if batchResolve dontMarginalizeVariablesAll!(fg) foreach(x->setSolvable!(fg, x, 1), setdiff(ls(fg),ls(fg,r"drt"))) foreach(x->setSolvable!(fg, x, 1), setdiff(lsf(fg),lsf(fg,r"drt"))) - tree = solveTree!(fg, maxparallel=1000) + tree = solveTree!(fg) end saveDFG(fg, resultsdir*"/racecar_fg_final") diff --git a/src/beamforming/SASBearing2D.jl b/src/beamforming/SASBearing2D.jl index 09e82e951..1ede717cf 100644 --- a/src/beamforming/SASBearing2D.jl +++ b/src/beamforming/SASBearing2D.jl @@ -41,7 +41,7 @@ mutable struct SASREUSE end -mutable struct SASBearing2D <: AbstractRelativeFactorMinimize +mutable struct SASBearing2D <: AbstractRelativeMinimize cfgTotal::CBFFilterConfig cfgLIE::CBFFilterConfig waveformsIn::Array{Complex{Float64}} diff --git a/src/images/Pose2AprilTag4Corners.jl b/src/images/Pose2AprilTag4Corners.jl index a6cf0ee63..7e7d0701b 100644 --- a/src/images/Pose2AprilTag4Corners.jl +++ b/src/images/Pose2AprilTag4Corners.jl @@ -226,10 +226,18 @@ end # just pass through sample and factor evaluations -getSample(pat4c::Pose2AprilTag4Corners, N::Int=1) = getSample(pat4c.Zij, N) - -(pat4c::Pose2AprilTag4Corners)(x...) = pat4c.Zij(x...) +getSample(pat4c::CalcFactor{<:Pose2AprilTag4Corners}, N::Int=1) = (rand(pat4c.factor.Zij.z, N), ) +function (pat4c::CalcFactor{<:Pose2AprilTag4Corners})(res::AbstractVector{<:Real}, + z, + wxi, + wxj) + # + wXjhat = SE2(wxi)*SE2(z) + jXjhat = SE2(wxj) \ wXjhat + se2vee!(res, jXjhat) + nothing +end ## serialization diff --git a/test/testPose2AprilTag4Corner.jl b/test/testPose2AprilTag4Corner.jl index f2ad59b72..f1377b44b 100644 --- a/test/testPose2AprilTag4Corner.jl +++ b/test/testPose2AprilTag4Corner.jl @@ -67,8 +67,8 @@ addVariable!(fg, :tag17, Pose2) atf = addFactor!(fg, [:x0;:tag17], apt4) - -meas = freshSamples(apt4,2) +meas = freshSamples(IIF._getCCW(atf),2) +# meas = freshSamples(fg, DFG.getLabel(atf),2) @test meas isa Tuple @test meas[1] isa Array