change to addVariable!(..)

examples/Hexagonal2D_BearingOnly_SLAM.jl

@@ -9,7 +9,7 @@ using RoME, Distributions
 fg = initfg()
 
 # Add the first pose :x0
-addNode!(fg, :x0, Pose2)
+addVariable!(fg, :x0, Pose2)
 
 # Add at a fixed location PriorPose2 to pin :x0 to a starting location
 addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*Matrix{Float64}(LinearAlgebra.I, 3,3))))
@@ -18,7 +18,7 @@ addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*Matrix{Float64}(LinearA
 for i in 0:12
     psym = Symbol("x$i")
     nsym = Symbol("x$(i+1)")
-    addNode!(fg, nsym, Pose2)
+    addVariable!(fg, nsym, Pose2)
     pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal( [0.1;0.1;0.1].^2 ) )))
     addFactor!(fg, [psym;nsym], pp )
 end
@@ -37,7 +37,7 @@ Gadfly.push_theme(:default) # light background, where Juno uses dark background
 Gadfly.draw(Gadfly.PDF("/tmp/test1.pdf", 20cm, 10cm),pl)  # or PNG(...)
 
 # Add a landmark l1
-addNode!(fg, :l1, Point2, labels=["LANDMARK"])
+addVariable!(fg, :l1, Point2, labels=["LANDMARK"])
 
 bear1 = atan(10,20)
 bear2 = atan(10,10) - pi/3

examples/Hexagonal2D_SLAM.jl

@@ -12,7 +12,7 @@ using RoMEPlotting
 fg = initfg()
 
 # Add the first pose :x0
-addNode!(fg, :x0, Pose2)
+addVariable!(fg, :x0, Pose2)
 
 # Add at a fixed location PriorPose2 to pin :x0 to a starting location (10,10, pi/4)
 addFactor!(fg, [:x0], IIF.Prior( MvNormal([10; 10; pi/6.0], Matrix(Diagonal([0.1;0.1;0.05].^2)) )))
@@ -21,7 +21,7 @@ addFactor!(fg, [:x0], IIF.Prior( MvNormal([10; 10; pi/6.0], Matrix(Diagonal([0.1
 for i in 0:5
     psym = Symbol("x$i")
     nsym = Symbol("x$(i+1)")
-    addNode!(fg, nsym, Pose2)
+    addVariable!(fg, nsym, Pose2)
     pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
     addFactor!(fg, [psym;nsym], pp )
 end
@@ -35,7 +35,7 @@ pl = drawPoses(fg)
 Gadfly.draw(Gadfly.PDF("/tmp/test1.pdf", 20cm, 10cm),pl)  # or PNG(...)
 
 # Add landmarks with Bearing range measurements
-addNode!(fg, :l1, Point2, labels=["LANDMARK"])
+addVariable!(fg, :l1, Point2, labels=["LANDMARK"])
 p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0))
 addFactor!(fg, [:x0; :l1], p2br)
 

examples/Hexagonal2D_SLAM_FixedLag.jl

@@ -20,21 +20,21 @@ lagLength = 30
 # Standard Hexagonal example for totalIterations - solve every iterationsPerSolve iterations.
 function runHexagonalExample(fg::FactorGraph, totalIterations::Int, iterationsPerSolve::Int)::DataFrame
     # Add the first pose :x0
-    addNode!(fg, :x0, Pose2)
+    addVariable!(fg, :x0, Pose2)
 
     # Add at a fixed location PriorPose2 to pin :x0 to a starting location
     addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*Matrix{Float64}(LinearAlgebra.I, 3,3))))
 
     # Add a landmark l1
-    addNode!(fg, :l1, Point2, labels=["LANDMARK"])
+    addVariable!(fg, :l1, Point2, labels=["LANDMARK"])
 
     # Drive around in a hexagon a number of times
     solveTimes = DataFrame(GraphSize = [], TimeBuildBayesTree = [], TimeSolveGraph = [])
     for i in 0:totalIterations
         psym = Symbol("x$i")
         nsym = Symbol("x$(i+1)")
         @info "Adding pose $nsym..."
-        addNode!(fg, nsym, Pose2)
+        addVariable!(fg, nsym, Pose2)
         pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal( [0.1;0.1;0.1].^2 ) )))
         @info "Adding odometry factor between $psym -> $nsym..."
         addFactor!(fg, [psym;nsym], pp )

examples/KinematicJoints.jl

@@ -63,14 +63,14 @@ N=100
 fg = RoME.initfg()
 
 # base
-addNode!(fg, :x1, dims=6)
+addVariable!(fg, :x1, dims=6)
 pos = PriorPose3(MvNormal(zeros(6),1e-6*Matrix{Float64}(LinearAlgebra.I, 6,6)))
 addFactor!(fg, [:x1], pos) # base
 initializeNode!(fg, :x1)
 
 
 # torso
-addNode!(fg, :x2, dims=6)
+addVariable!(fg, :x2, dims=6)
 hip = ZJoint(Normal(pi/3,0.1))
 addFactor!(fg, [:x1, :x2], hip) # hio
 initializeNode!(fg, :x2)
@@ -111,7 +111,7 @@ plotPose3Pairs(fg, :x2)
 
 
 # torso
-addNode!(fg, :x3, dims=6)
+addVariable!(fg, :x3, dims=6)
 should = XJoint(Normal(pi/4,0.1))
 addFactor!(fg, [:x2, :x3], should) # hio
 initializeNode!(fg, :x3)
@@ -129,7 +129,7 @@ solveandvisualize(fg, vis)
 
 
 # upper arm
-addNode!(fg, :x4, dims=6)
+addVariable!(fg, :x4, dims=6)
 should = XJoint(Normal(pi/4,0.1))
 addFactor!(fg, [:x3, :x4], should) # hio
 initializeNode!(fg, :x4)
@@ -156,19 +156,19 @@ N=300
 fg = RoME.initfg()
 
 # base
-addNode!(fg, :x1, dims=6)
+addVariable!(fg, :x1, dims=6)
 pos = PriorPose3(MvNormal(zeros(6),1e-6*Matrix{Float64}(LinearAlgebra.I, 6,6)))
 addFactor!(fg, [:x1], pos) # base
 initializeNode!(fg, :x1)
 
 
-addNode!(fg, :x2, dims=6)
+addVariable!(fg, :x2, dims=6)
 hip = ZJoint(Uniform(-pi/3,pi/3))
 addFactor!(fg, [:x1, :x2], hip) # hio
 initializeNode!(fg, :x2)
 
 
-addNode!(fg, :x3, dims=6)
+addVariable!(fg, :x3, dims=6)
 should = XJoint(Uniform(-pi/4,pi/4))
 addFactor!(fg, [:x2, :x3], should) # hio
 initializeNode!(fg, :x3)

examples/MultimodalRangeBearing.jl

@@ -17,15 +17,15 @@ fg = initfg(sessionname="MULTIMODAL_2D_TUTORIAL")
 
 
 # Add landmarks with Bearing range measurements
-addNode!(fg, :l1, Point2, labels=["LANDMARK"])
-addNode!(fg, :l2, Point2, labels=["LANDMARK"])
+addVariable!(fg, :l1, Point2, labels=["LANDMARK"])
+addVariable!(fg, :l2, Point2, labels=["LANDMARK"])
 
 addFactor!(fg, [:l1], Prior(MvNormal([10.0;0.0], Matrix(Diagonal([1.0;1.0].^2)))) )
 addFactor!(fg, [:l2], Prior(MvNormal([30.0;0.0], Matrix(Diagonal([1.0;1.0].^2)))) )
 
 setVal!(getVert(fg, :l2), zeros(2,100))
 
-addNode!(fg, :x0, Pose2)
+addVariable!(fg, :x0, Pose2)
 # addFactor!(fg, [:x0], Prior(MvNormal([0.0;0.0;0], Matrix(Diagonal([1.0;1.0;0.01].^2)))) )
 
 
@@ -66,6 +66,6 @@ X0pts = getPoints(X0)
 # for i in 0:5
 #   psym = Symbol("x$i")
 #   nsym = Symbol("x$(i+1)")
-#   addNode!(fg, nsym, Pose2, labels=["POSE"])
+#   addVariable!(fg, nsym, Pose2, labels=["POSE"])
 #   addFactor!(fg, [psym;nsym], Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2)))))
 # end

examples/RangesExample.jl

@@ -34,12 +34,12 @@ GTl[:l4] = [120.0;-50]
 fg = initfg()
 
 # first pose with no initial estimate
-addNode!(fg, :l100, Point2)
+addVariable!(fg, :l100, Point2)
 
 # add three landmarks
-addNode!(fg, :l1, Point2)
-addNode!(fg, :l2, Point2)
-addNode!(fg, :l3, Point2)
+addVariable!(fg, :l1, Point2)
+addVariable!(fg, :l2, Point2)
+addVariable!(fg, :l3, Point2)
 
 # and put priors on :l101 and :l102
 addFactor!(fg, [:l1;], PriorPoint2(MvNormal(GTl[:l1], Matrix{Float64}(LinearAlgebra.I, 2,2))))
@@ -98,7 +98,7 @@ function vehicle_drives_to!(fgl::FactorGraph, pos_sym::Symbol, GTp::Dict, GTl::D
   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.")
-    addNode!(fgl, pos_sym, Point2)
+    addVariable!(fgl, pos_sym, Point2)
     @show rho = norm(GTp[prev_sym] - GTp[pos_sym])
     ppr = Point2DPoint2DRange([rho], 3.0, [1.0])
     addFactor!(fgl, [prev_sym;pos_sym], ppr)
@@ -113,7 +113,7 @@ function vehicle_drives_to!(fgl::FactorGraph, pos_sym::Symbol, GTp::Dict, GTl::D
       ppr = Point2DPoint2DRange([rho], 3.0, [1.0])
       if !(ll in currvar)
         println("Adding variable vertex $ll, not yet in fgl::FactorGraph.")
-        addNode!(fgl, ll, Point2)
+        addVariable!(fgl, ll, Point2)
       end
       addFactor!(fgl, [pos_sym;ll], ppr)
     end

examples/RangesExampleTwo.jl

@@ -74,7 +74,7 @@ function isInFG!(fgl::FactorGraph, lbl::Symbol; N=100, ready=0)
 	v = nothing
 	if !haskey(fgl.IDs, lbl)
 		init = 300*randn(2,N)
-		v = addNode!(fgl, lbl, Point2, N=N, ready=ready)
+		v = addVariable!(fgl, lbl, Point2, N=N, ready=ready)
 	else
 		v = getVert(fgl, lbl)
 	end
@@ -101,7 +101,7 @@ function addNewPose!(fgl::FactorGraph,
                      N=N  )
   #
   init = 300*randn(2,N)
-  v = addNode!(fgl, lbl, Point2, N=N, ready=ready)
+  v = addVariable!(fgl, lbl, Point2, N=N, ready=ready)
   rhoZ = norm(GTp[string(lbl)]-GTp[string(from)])
   ppr = Point2DPoint2DRange([rhoZ], 3.0, [1.0])
   f = addFactor!(fgl, [from,lbl], ppr, ready=ready)
@@ -434,7 +434,7 @@ fg = initfg()
 
 # Some starting position
 init = 300*randn(2,N)
-v1 = addNode!(fg, :l100, Point2, N=N, ready=0)
+v1 = addVariable!(fg, :l100, Point2, N=N, ready=0)
 
 
 lmv1 = landmsInRange(GTl, GTp["l100"])

examples/SeparateInitializationProcess.jl

@@ -36,21 +36,21 @@ fg = initfg(sessionname="SLAM2D_TUTORIAL")
 
 
 # also add a PriorPose2 to pin the first pose at a fixed location
-addNode!(fg, :x0, Pose2, labels=["POSE"])
+addVariable!(fg, :x0, Pose2, labels=["POSE"])
 addFactor!(fg, [:x0], Prior(MvNormal([0.0;0.0;0], Matrix(Diagonal([1.0;1.0;0.01].^2)))) )
 
 # Drive around in a hexagon
 for i in 0:5
   psym = Symbol("x$i")
   nsym = Symbol("x$(i+1)")
-  addNode!(fg, nsym, Pose2)
+  addVariable!(fg, nsym, Pose2)
   addFactor!(fg, [psym;ensureAllInitialized!nsym], Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2)))))
 end
 
 # Graphs.plot(fg.g)
 
 # Add landmarks with Bearing range measurements
-addNode!(fg, :l1, Point2, labels=["LANDMARK"])
+addVariable!(fg, :l1, Point2, labels=["LANDMARK"])
 p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0))
 addFactor!(fg, [:x0; :l1], p2br)
 

src/RobotUtils.jl

@@ -191,8 +191,8 @@ function addOdoFG!(
         XnextInit[:,i] = addPose2Pose2(X[:,i], DX + ent)
     end
 
-    v = addNode!(fg, n, Pose2, N=N, ready=ready, labels=[labels;"POSE"])
-    # v = addNode!(fg, n, XnextInit, cov, N=N, ready=ready, labels=labels)
+    v = addVariable!(fg, n, Pose2, N=N, ready=ready, labels=[labels;"POSE"])
+    # v = addVariable!(fg, n, XnextInit, cov, N=N, ready=ready, labels=labels)
     pp = Pose2Pose2(MvNormal(DX, cov)) #[prev;v],
     f = addFactor!(fg, [prev;v], pp, ready=ready, autoinit=true )
     infor = inv(cov^2)
@@ -211,7 +211,7 @@ function addOdoFG!(
   # DX=Z.μ
   # cov=Z.Σ.mat
   vprev, X, nextn = getLastPose(fgl)
-  vnext = addNode!(fgl, nextn, Pose3, ready=ready, labels=labels)
+  vnext = addVariable!(fgl, nextn, Pose3, ready=ready, labels=labels)
   fact = addFactor!(fgl, [vprev;vnext], Z, autoinit=true)
 
   return vnext, fact
@@ -239,8 +239,8 @@ function addOdoFG!(
     if N==0
       N = size(X,2)
     end
-    # vnext = addNode!(fgl, nextn, X⊕odo, ones(1,1), N=N, ready=ready, labels=labels)
-    vnext = addNode!(fgl, nextn, Pose2, N=N, ready=ready, labels=labels)
+    # vnext = addVariable!(fgl, nextn, X⊕odo, ones(1,1), N=N, ready=ready, labels=labels)
+    vnext = addVariable!(fgl, nextn, Pose2, N=N, ready=ready, labels=labels)
     fact = addFactor!(fgl, [vprev;vnext], odo, autoinit=true)
 
     return vnext, fact
@@ -274,18 +274,18 @@ function initFactorGraph!(fg::FactorGraph;
       init = init!=nothing ? init : zeros(3)
       P0 = P0!=nothing ? P0 : Matrix(Diagonal([0.03;0.03;0.001]))
       # init = vectoarr2(init)
-      addNode!(fg,lbl,Pose2,N=N,autoinit=true,ready=ready,labels=String["VARIABLE"; labels] )
+      addVariable!(fg,lbl,Pose2,N=N,autoinit=true,ready=ready,labels=String["VARIABLE"; labels] )
       push!(nodesymbols, lbl)
-      # v1 = addNode!(fg, lbl, init, P0, N=N, ready=ready, labels=labels)
+      # v1 = addVariable!(fg, lbl, init, P0, N=N, ready=ready, labels=labels)
       fctVert = addFactor!(fg, [lbl;], PriorPose2(MvNormal(init, P0)), ready=ready, labels=String["FACTOR"; labels]) #[v1],
       push!(nodesymbols, Symbol(fctVert.label))
   end
   if firstPoseType == Pose3
       init = init!=nothing ? init : zeros(6)
       P0 = P0!=nothing ? P0 : Matrix(Diagonal([0.03;0.03;0.03;0.001;0.001;0.001]))
-      addNode!(fg,lbl,Pose2,N=N,autoinit=true,ready=ready,labels=String["VARIABLE"; labels] )
+      addVariable!(fg,lbl,Pose2,N=N,autoinit=true,ready=ready,labels=String["VARIABLE"; labels] )
       push!(nodesymbols, lbl)
-      # v1 = addNode!(fg, lbl, init, P0, N=N, ready=ready, labels=labels)
+      # v1 = addVariable!(fg, lbl, init, P0, N=N, ready=ready, labels=labels)
       fctVert = addFactor!(fg, [lbl;], PriorPose3(MvNormal(init, P0)), ready=ready, labels=String["FACTOR"; labels]) #[v1],
       push!(nodesymbols, Symbol(fctVert.label))
   end
@@ -296,9 +296,9 @@ end
 function newLandm!(fg::FactorGraph, lm::T, wPos::Array{Float64,2}, sig::Array{Float64,2};
                   N::Int=100, ready::Int=1, labels::Vector{T}=String[]) where {T <: AbstractString}
 
-    vert=addNode!(fg, Symbol(lm), Point2, N=N, ready=ready, labels=union(["LANDMARK";], labels))
+    vert=addVariable!(fg, Symbol(lm), Point2, N=N, ready=ready, labels=union(["LANDMARK";], labels))
     # TODO -- need to confirm this function is updating the correct memory location. v should be pointing into graph
-    # vert=addNode!(fg, Symbol(lm), wPos, sig, N=N, ready=ready, labels=labels)
+    # vert=addVariable!(fg, Symbol(lm), wPos, sig, N=N, ready=ready, labels=labels)
 
     vert.attributes["age"] = 0
     vert.attributes["maxage"] = 0
@@ -736,7 +736,7 @@ function addLinearArrayConstraint(fgl::FactorGraph,
   if !haskey(fgl.IDs, landm)
     pts = getVal(fgl, pose) + cl
     N = size(pts,2)
-    vl1 = addNode!(fgl, landm,  pts,  N=N)
+    vl1 = addVariable!(fgl, landm,  pts,  N=N)
     println("Automatically added $(landm) to the factor graph")
   end
   addFactor!(fgl, [getVert(fgl, pose); getVert(fgl, landm)], cl)

src/Slam.jl

@@ -24,8 +24,8 @@ function addOdoFG!(slaml::SLAMWrapper, odo::Pose3Pose3;
   # vprev, X, nextn = getLastPose(fgl)
   npnum = parse(Int,string(slaml.lastposesym)[2:end]) + 1
   nextn = Symbol("x$(npnum)")
-  vnext = addNode!(slaml.fg, nextn, Pose2(labels=["POSE";]), N=N, ready=ready)
-  # vnext = addNode!(slaml.fg, nextn, getVal(vprev) ⊕ odo, N=N, ready=ready, labels=["POSE"])
+  vnext = addVariable!(slaml.fg, nextn, Pose2(labels=["POSE";]), N=N, ready=ready)
+  # vnext = addVariable!(slaml.fg, nextn, getVal(vprev) ⊕ odo, N=N, ready=ready, labels=["POSE"])
   slaml.lastposesym = nextn
   fact = addFactor!(slaml.fg, [vprev;vnext], odo)
 
@@ -50,8 +50,8 @@ function addposeFG!(slaml::SLAMWrapper,
   # preinit
   vnext = nothing
   if !haskey(slaml.fg.IDs, nextn)
-    vnext = addNode!(slaml.fg, nextn, Pose2, N=N, ready=ready)
-    # vnext = addNode!(slaml.fg, nextn, getVal(vprev), N=N, ready=ready, labels=["POSE"])
+    vnext = addVariable!(slaml.fg, nextn, Pose2, N=N, ready=ready)
+    # vnext = addVariable!(slaml.fg, nextn, getVal(vprev), N=N, ready=ready, labels=["POSE"])
   else
     vnext = getVert(slaml.fg, nextn) #, api=localapi # as optimization, assuming we already have latest vnest in slaml.fg
   end

test/TestPoseAndPoint2Constraints.jl

@@ -17,7 +17,7 @@ global initCov = Matrix(Diagonal([0.03;0.03;0.001]))
 global odoCov = Matrix(Diagonal([3.0;3.0;0.01]))
 
 # Some starting position
-global v1 = addNode!(fg, :x0, Pose2, N=N)
+global v1 = addVariable!(fg, :x0, Pose2, N=N)
 global initPosePrior = PriorPose2(MvNormal(zeros(3), initCov))
 global f1  = addFactor!(fg,[v1], initPosePrior)
 
@@ -26,7 +26,7 @@ global f1  = addFactor!(fg,[v1], initPosePrior)
 @test Pose2Pose2(randn(2), Matrix{Float64}(LinearAlgebra.I, 2,2),[1.0;]) != nothing
 
 # and a second pose
-global v2 = addNode!(fg, :x1, Pose2, N=N)
+global v2 = addVariable!(fg, :x1, Pose2, N=N)
 global ppc = Pose2Pose2([50.0;0.0;pi/2], odoCov, [1.0])
 global f2 = addFactor!(fg, [:x0;:x1], ppc)
 
@@ -53,7 +53,7 @@ global pts = getVal(fg, :x1)
 
 
 # check that yaw is working
-global v3 = addNode!(fg, :x2, Pose2, N=N)
+global v3 = addVariable!(fg, :x2, Pose2, N=N)
 global ppc = Pose2Pose2([50.0;0.0;0.0], odoCov, [1.0])
 global f3 = addFactor!(fg, [v2;v3], ppc)
 
@@ -77,7 +77,7 @@ global pts = getVal(fg, :x2)
 println("test bearing range evaluations")
 
 # new landmark
-global l1 = addNode!(fg, :l1, Point2, N=N)
+global l1 = addVariable!(fg, :l1, Point2, N=N)
 # and pose to landmark constraint
 global rhoZ1 = norm([10.0;0.0])
 global ppr = Pose2Point2BearingRange(Uniform(-pi,pi),Normal(rhoZ1,1.0))