diff --git a/examples/Beehive2D_WithoutInit.jl b/examples/Beehive2D_WithoutInit.jl index 0a04a4e4..03e00798 100644 --- a/examples/Beehive2D_WithoutInit.jl +++ b/examples/Beehive2D_WithoutInit.jl @@ -28,7 +28,7 @@ function driveHex(fgl, posecount::Int) end -function offsetHexLeg(fgl::FactorGraph, posecount::Int; direction=:right) +function offsetHexLeg(fgl::AbstractDFG, posecount::Int; direction=:right) psym = Symbol("x$(posecount-1)") nsym = Symbol("x$(posecount)") posecount += 1 diff --git a/examples/Hexagonal2D_SLAM_FixedLag.jl b/examples/Hexagonal2D_SLAM_FixedLag.jl index 76e042b4..d6531bce 100644 --- a/examples/Hexagonal2D_SLAM_FixedLag.jl +++ b/examples/Hexagonal2D_SLAM_FixedLag.jl @@ -18,7 +18,7 @@ solveEveryNVariables = 20 lagLength = 30 # Standard Hexagonal example for totalIterations - solve every iterationsPerSolve iterations. -function runHexagonalExample(fg::FactorGraph, totalIterations::Int, iterationsPerSolve::Int)::DataFrame +function runHexagonalExample(fg::AbstractDFG, totalIterations::Int, iterationsPerSolve::Int)::DataFrame # Add the first pose :x0 addVariable!(fg, :x0, Pose2) diff --git a/examples/RangesExample.jl b/examples/RangesExample.jl index 2578276e..5a22e97c 100644 --- a/examples/RangesExample.jl +++ b/examples/RangesExample.jl @@ -77,7 +77,7 @@ function vehicle_drives_to!(fgl::G, pos_sym::Symbol, GTp::Dict, GTl::Dict; measu currvar = ls(fgl) prev_sym = Symbol("l$(maximum(Int[parse(Int,string(currvar[i])[2:end]) for i in 2:length(currvar)]))") if !(pos_sym in currvar) - println("Adding variable vertex $pos_sym, not yet in fgl::FactorGraph.") + 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, 3.0) ) @@ -92,7 +92,7 @@ function vehicle_drives_to!(fgl::G, pos_sym::Symbol, GTp::Dict, GTl::Dict; measu if rho < measurelimit ppr = Point2Point2Range( Normal(rho, 3.0) ) if !(ll in currvar) - println("Adding variable vertex $ll, not yet in fgl::FactorGraph.") + println("Adding variable vertex $ll, not yet in fgl::AbstractDFG.") addVariable!(fgl, ll, Point2) end addFactor!(fgl, [pos_sym;ll], ppr) diff --git a/examples/RangesExampleTwo.jl b/examples/RangesExampleTwo.jl index fbfa3ad0..9c8e19c3 100644 --- a/examples/RangesExampleTwo.jl +++ b/examples/RangesExampleTwo.jl @@ -62,7 +62,7 @@ function landmsInRange(GTl::Dict{String, Vector{Float64}}, cur::Vector{Float64}; return inrange end -function isInFG!(fgl::FactorGraph, lbl::Symbol; N=100, solvable=0) +function isInFG!(fgl::AbstractDFG, lbl::Symbol; N=100, solvable=0) v = nothing if !haskey(fgl.IDs, lbl) init = 300*randn(2,N) @@ -73,7 +73,7 @@ function isInFG!(fgl::FactorGraph, lbl::Symbol; N=100, solvable=0) v end -function addLandmsOnPose!(fgl::FactorGraph, pose::Graphs.ExVertex, GTl::Dict{String, Float64}; +function addLandmsOnPose!(fgl::AbstractDFG, pose::Graphs.ExVertex, GTl::Dict{String, Float64}; solvable=0,N=100) for gtl in GTl println("addLandmsOnPose! -- adding $(gtl[1])") @@ -85,7 +85,7 @@ function addLandmsOnPose!(fgl::FactorGraph, pose::Graphs.ExVertex, GTl::Dict{Str nothing end -function addNewPose!(fgl::FactorGraph, +function addNewPose!(fgl::AbstractDFG, from::Symbol, lbl::Symbol, GTp; @@ -101,7 +101,7 @@ function addNewPose!(fgl::FactorGraph, getVert(fgl, lbl) end -function drive(fgl::FactorGraph, GTp, GTl, from, to; N=100) +function drive(fgl::AbstractDFG, GTp, GTl, from, to; N=100) v = addNewPose!(fgl, from, to, GTp, N=N) addLandmsOnPose!(fgl, v, landmsInRange(GTl, GTp[string(to)], lim=120.0), N=N ) println("added landmark") @@ -129,7 +129,7 @@ function drawQuadLandms(fgl; end -function drawLandmMargOver(fgl::FactorGraph, lbl::String, +function drawLandmMargOver(fgl::AbstractDFG, lbl::String, file, gtlayers; w=30cm, h=20cm) @@ -373,7 +373,7 @@ function drawIllustrations(GTl, GTp, folderloc) end -function evaluateAccuracy(fgl::FactorGraph, GTp) +function evaluateAccuracy(fgl::AbstractDFG, GTp) # get last pose xx,ll = ls(fg) maxPosition = 0 @@ -390,7 +390,7 @@ function evaluateAccuracy(fgl::FactorGraph, GTp) return postlikeli, norm(MaxPointError) end -function evaluateAllAccuracy(fgl::FactorGraph, GTp, GTl) +function evaluateAllAccuracy(fgl::AbstractDFG, GTp, GTl) xx,ll = ls(fg) LK = Float64[] PE = Float64[] diff --git a/examples/dev/ISAMRemoteSolve.jl b/examples/dev/ISAMRemoteSolve.jl index 5201a357..c02aee72 100644 --- a/examples/dev/ISAMRemoteSolve.jl +++ b/examples/dev/ISAMRemoteSolve.jl @@ -16,7 +16,7 @@ function addLandmMeasRemote(frm,to,DX,cov) nothing end -function requestAllPosesLandmsRemote(fg::FactorGraph) +function requestAllPosesLandmsRemote(fg::AbstractDFG) if !ENABLISAMREF return nothing end dict = Dict{Int,Array{Float64,1}}() xx,ll=ls(fg) @@ -76,7 +76,7 @@ function doISAMSolve(d,f;toT=Inf, savejld=false, retfg=false, MM=Union{}, return !retfg ? gtvals : gtvals, fgu end -function computeGraphResiduals(fgl::FactorGraph, dict::Dict{Int,Array{Float64,1}}) +function computeGraphResiduals(fgl::AbstractDFG, dict::Dict{Int,Array{Float64,1}}) X,Y,Th,LB = get2DPoseMax(fgl) XL,YL,ThL,LBL = get2DLandmMax(fgl) @@ -127,7 +127,7 @@ function drawUnimodalLandm(LBL, Xpp, Ypp;c="red", lbls=true) return psplt end -function convertISAMDictArr(fgu::FactorGraph, isamdict::Dict{Int,Array{Float64,1}}) +function convertISAMDictArr(fgu::AbstractDFG, isamdict::Dict{Int,Array{Float64,1}}) X=Float64[];Y=Float64[];Th=Float64[]; Xl=Float64[];Yl=Float64[]; @@ -148,7 +148,7 @@ function convertISAMDictArr(fgu::FactorGraph, isamdict::Dict{Int,Array{Float64,1 return xx, X, Y, Th, ll, Xl, Yl end -function drawUniPosesLandm(fgu::FactorGraph, isamdict::Dict{Int,Array{Float64,1}}) +function drawUniPosesLandm(fgu::AbstractDFG, isamdict::Dict{Int,Array{Float64,1}}) xx, X, Y, Th, ll, Xl, Yl = convertISAMDictArr(fgu, isamdict) @@ -161,7 +161,7 @@ function drawUniPosesLandm(fgu::FactorGraph, isamdict::Dict{Int,Array{Float64,1} return p end -function drawCompPosesLandm(fgl::FactorGraph,isamdict::Dict{Int,Array{Float64,1}},fgu::FactorGraph; +function drawCompPosesLandm(fgl::AbstractDFG,isamdict::Dict{Int,Array{Float64,1}},fgu::AbstractDFG; lbls=true, drawhist=false, drawunilm=true) p=drawPosesLandms(fgl,lbls=lbls,drawhist=drawhist) diff --git a/examples/dev/TreeDescriptor.jl b/examples/dev/TreeDescriptor.jl index 8356f179..2b27bdca 100644 --- a/examples/dev/TreeDescriptor.jl +++ b/examples/dev/TreeDescriptor.jl @@ -12,7 +12,7 @@ end treelms = Dict{Int,TreeDescriptor}() # get the mean trajector index from sighing to this landmark -function getLandmTrajIndxs(fgl::FactorGraph, lmlbl::String) +function getLandmTrajIndxs(fgl::AbstractDFG, lmlbl::String) vlm = getVert(fgl,lmlbl) psoi = String[] @@ -28,7 +28,7 @@ function getLandmTrajIndxs(fgl::FactorGraph, lmlbl::String) end # take mean of poses connected to this landmark's index numbers -function getLandmMeanTrajIndx(fgl::FactorGraph, lmlbl::String) +function getLandmMeanTrajIndx(fgl::AbstractDFG, lmlbl::String) psoi = getLandmTrajIndxs(fgl, lmlbl) idxs = Float64[] for ps in psoi @@ -37,7 +37,7 @@ function getLandmMeanTrajIndx(fgl::FactorGraph, lmlbl::String) return Base.mean(idxs) end -function getAllLandmMeanTrajIndx(fgl::FactorGraph, LBS::Array{String,1}) +function getAllLandmMeanTrajIndx(fgl::AbstractDFG, LBS::Array{String,1}) MIDX = Float64[] for i in 1:length(LBS) push!(MIDX, getLandmMeanTrajIndx(fgl, LBS[i]) ) @@ -66,11 +66,11 @@ function getAllLandmNearByIndx(LB, X, Y, MIDX, nearidx; return nnh end -function calcFeatDesc(fgl::FactorGraph, flbl::String, midx::Float64) +function calcFeatDesc(fgl::AbstractDFG, flbl::String, midx::Float64) lm = getVert(fgl,flbl) end -function calcLandmDescriptions!(fgl::FactorGraph, lmd::Dict{Int,TreeDescriptor}) +function calcLandmDescriptions!(fgl::AbstractDFG, lmd::Dict{Int,TreeDescriptor}) X,Y,th,LB = get2DLandmMeans(fgl) getAllLandmNearByIndx(LB,X,Y) MIDX = getAllLandmMeanTrajIndx(fgl, LB) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 911f721d..bae878b9 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -57,8 +57,8 @@ function addposeFG!(slaml::SLAMWrapper, end slaml.lastposesym = nextn - addsubtype(fgl::FactorGraph, vprev, vnext, cc::IncrementalInference.FunctorPairwise) = addFactor!(fgl, [vprev;vnext], cc) - addsubtype(fgl::FactorGraph, vprev, vnext, cc::IncrementalInference.FunctorSingleton) = addFactor!(fgl, [vnext], cc) + addsubtype(fgl::AbstractDFG, vprev, vnext, cc::IncrementalInference.FunctorPairwise) = addFactor!(fgl, [vprev;vnext], cc) + addsubtype(fgl::AbstractDFG, vprev, vnext, cc::IncrementalInference.FunctorSingleton) = addFactor!(fgl, [vnext], cc) facts = Graphs.ExVertex[] PP = BallTreeDensity[] @@ -466,17 +466,17 @@ function getNextLbl(fgl::AbstractDFG, chr) end -function getLastPose(fgl::FactorGraph) +function getLastPose(fgl::AbstractDFG) return getNextLbl(fgl, 'x') end -getLastPose2D(fgl::FactorGraph) = getLastPose(fgl) +getLastPose2D(fgl::AbstractDFG) = getLastPose(fgl) function getlastpose(slam::SLAMWrapper) error("getlastpose -- Not implemented yet") end -function getLastLandm2D(fgl::FactorGraph) +function getLastLandm2D(fgl::AbstractDFG) return getNextLbl(fgl, 'l') end diff --git a/src/FactorGraphAnalysisTools.jl b/src/FactorGraphAnalysisTools.jl index c0d33545..3202f7f0 100644 --- a/src/FactorGraphAnalysisTools.jl +++ b/src/FactorGraphAnalysisTools.jl @@ -1,13 +1,13 @@ # Graph analysis tools -function rangeErrMaxPoint2(fgl1::FactorGraph, id1, fgl2::FactorGraph ,id2) +function rangeErrMaxPoint2(fgl1::AbstractDFG, id1, fgl2::AbstractDFG ,id2) mv1 = getKDEMax(getVertKDE(fgl1,id1)) mv2 = getKDEMax(getVertKDE(fgl2,id2)) return norm(mv1[1:2]-mv2[1:2]) end -function rangeCompAllPoses(fgl1::FactorGraph, fgl2::FactorGraph) +function rangeCompAllPoses(fgl1::AbstractDFG, fgl2::AbstractDFG) ranges = Float64[] xx,ll = ls(fgl1) for x in xx @@ -18,8 +18,8 @@ end function rangeCompAllPoses( valsbaseline::Dict{Int,Array{Float64,1}}, - fglbaseline::FactorGraph, - fgltest::FactorGraph) + fglbaseline::AbstractDFG, + fgltest::AbstractDFG) ranges = Float64[] xx,ll = ls(fgltest) diff --git a/src/RobotUtils.jl b/src/RobotUtils.jl index 091cc27e..51efd5e3 100644 --- a/src/RobotUtils.jl +++ b/src/RobotUtils.jl @@ -305,7 +305,7 @@ constraint factor are returned as a tuple. """ function addOdoFG!( - fgl::FactorGraph, + fgl::AbstractDFG, odo::Pose2Pose2; N::Int=0, solvable::Int=1, @@ -322,41 +322,20 @@ function addOdoFG!( return vnext, fact end -# """ -# $SIGNATURES -# -# Initialize an empty `::FactorGraph` object while initializing `sessionname`, `robotname`, and `cloudgraph`. -# """ -# function initfg(;sessionname="NA",robotname="",username="",cloudgraph=nothing) -# # fgl = RoME.initfg(sessionname=sessionname) -# fgl = IIF.FactorGraph() -# fgl.sessionname = sessionname -# fgl.robotname = robotname -# fgl.username = username -# fgl.cg = cloudgraph -# return fgl -# end -# function initfg(;sessionname="NA") -# fgl = IIF.FactorGraph() -# fgl.sessionname=sessionname -# # fgl.robotname = ? -# # registerCallback!(fgl, RoME.getSample) # RoME.evalPotention -# return fgl -# end """ $(SIGNATURES) Initialize a factor graph object as Pose2, Pose3, or neither and returns variable and factor symbols as array. """ -function initFactorGraph!(fg::G; +function initFactorGraph!(fg::AbstractDFG; P0::Union{Array{Float64,2},Nothing}=nothing, init::Union{Vector{Float64},Nothing}=nothing, N::Int=100, lbl::Symbol=:x0, solvable::Int=1, firstPoseType=Pose2, - labels::Vector{Symbol}=Symbol[]) where G <: AbstractDFG + labels::Vector{Symbol}=Symbol[]) # nodesymbols = Symbol[] if firstPoseType == Pose2 @@ -382,7 +361,7 @@ function initFactorGraph!(fg::G; end -function newLandm!(fg::FactorGraph, lm::T, wPos::Array{Float64,2}, sig::Array{Float64,2}; +function newLandm!(fg::AbstractDFG, lm::T, wPos::Array{Float64,2}, sig::Array{Float64,2}; N::Int=100, solvable::Int=1, labels::Vector{T}=String[]) where {T <: AbstractString} vert=addVariable!(fg, Symbol(lm), Point2, N=N, solvable=solvable, labels=union(["LANDMARK";], labels)) diff --git a/src/SimulationUtils.jl b/src/SimulationUtils.jl index 09d69a87..2d720e39 100644 --- a/src/SimulationUtils.jl +++ b/src/SimulationUtils.jl @@ -1,6 +1,6 @@ # And 2xN matrix of N landmark xy positions as variable nodes in factor graph -function addSimMapFG!(fg::FactorGraph, lms::Array{Float64,2}) +function addSimMapFG!(fg::AbstractDFG, lms::Array{Float64,2}) for i in 1:size(lms,2) newLandm!(fg,string('l',i), vectoarr2(lms[:,i]), 0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)) end @@ -8,7 +8,7 @@ function addSimMapFG!(fg::FactorGraph, lms::Array{Float64,2}) end # assume all the landmarks are already loaded into the Ground Truth FG -function simOdo!(fgGT::FactorGraph, fg::FactorGraph, DX::Array{Float64,1}; +function simOdo!(fgGT::AbstractDFG, fg::AbstractDFG, DX::Array{Float64,1}; noiserate=2.0*[3e-2;3e-2;1.5e-3], driftrate=[0.0;0.0;0.0], detLM=Union{}) prev, X, nextn = getLastPose2D(fg) addOdoFG!(fgGT, nextn, DX, 0.001*Matrix{Float64}(LinearAlgebra.I, 3,3)) @@ -25,7 +25,7 @@ function simOdo!(fgGT::FactorGraph, fg::FactorGraph, DX::Array{Float64,1}; end -function truePredBR(fgGT::FactorGraph, fg::FactorGraph, ps::String, lm::String) +function truePredBR(fgGT::AbstractDFG, fg::AbstractDFG, ps::String, lm::String) trubr = predictBodyBR(fgGT, ps, lm) truA = [trubr[1]; trubr[2]] # Prediction of BR measurement @@ -61,14 +61,14 @@ function calcPosePointBearingRange(pose::Vector{<:Real}, end -function showTruePredBR(fgGT::FactorGraph, fg::FactorGraph, ps::String, lm::String, cov::Array{Float64,2}) +function showTruePredBR(fgGT::AbstractDFG, fg::AbstractDFG, ps::String, lm::String, cov::Array{Float64,2}) truA, preA = truePredBR(fgGT, fg, ps, lm) measA = truA + [cov[1,1]*randn();cov[2,2]*randn()] mala = malahanobisBR(measA, preA, cov) println(ps,lm, ": true BR=$(round(truA,digits=3)), pred BR=$(round(preA,digits=3)), mala=$(round(mala,digits=3))") end -function crossMalaBR(fgGT::FactorGraph, fg::FactorGraph, +function crossMalaBR(fgGT::AbstractDFG, fg::AbstractDFG, ps::String, lmT::String, lmE, cov::Array{Float64,2}) truT, preT = truePredBR(fgGT, fg, ps, lmT) measT = truT + [cov[1,1]*randn();cov[2,2]*randn()] diff --git a/src/Slam.jl b/src/Slam.jl index 61f5ae87..85d37d5f 100644 --- a/src/Slam.jl +++ b/src/Slam.jl @@ -24,7 +24,7 @@ DevNotes - TODO consolidate with SLAMWrapperLocal """ mutable struct SLAMWrapper <: AbstractSLAM - fg::IncrementalInference.FactorGraph + fg::AbstractDFG tree::NothingUnion{IncrementalInference.BayesTree} lndmidx::Int lastposesym::Symbol