Merge pull request #295 from JuliaRobotics/master

v0.7.1-rc1

.travis.yml

@@ -22,14 +22,13 @@ jobs:
   allow_failures:
     - julia: nightly
     - arch: arm64
-    - name: "ALL MASTERS"
   include:
     - name: "ALL MASTERS"
       julia: 1.5
       script:
         - if [[ -a .git/shallow ]]; then git fetch --unshallow; fi
         - julia -e 'using Pkg; pkg"add DistributedFactorGraphs#master"; pkg"add IncrementalInference#master";'
-        - julia --check-bounds=yes -e 'using Pkg; Pkg.test("Caesar"; coverage=false)'
+        - julia --check-bounds=yes -e 'using Pkg; Pkg.test("RoME"; coverage=false)'
       after_success: skip
     - name: "ARM64"
       arch: arm64

Project.toml

@@ -2,7 +2,7 @@ name = "RoME"
 uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 keywords = ["SLAM", "state-estimation", "mm-iSAM", "inference", "robotics"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.7.0"
+version = "0.7.1"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

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

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)
 

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)

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[]

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)
 

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)

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
 

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)

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))

src/SimulationUtils.jl

@@ -1,14 +1,14 @@
 
 # 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
     nothing
 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()]

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