Merge pull request #258 from JuliaRobotics/maintenance/prep030

addNode! becomes addVariable!

Author: dehann

REQUIRE

@@ -1,11 +1,12 @@
 julia 0.7 1.1
-RoME 0.3.0
-IncrementalInference 0.5.0
-Graphs 0.10.2
-KernelDensityEstimate 0.5.0
 Distributions 0.16.0
-TransformUtils 0.2.2
 Rotations 0.8.0
+Graphs 0.10.2
+KernelDensityEstimate 0.5.0 0.6.0
+ApproxManifoldProducts 0.0.3 0.2.0
+IncrementalInference 0.5.1 0.6.0
+RoME 0.3.1 0.4.0
+TransformUtils 0.2.2
 CoordinateTransformations 0.5.0
 JLD2
 JSON 0.18.0

docs/src/concepts/arena_visualizations.md

@@ -88,9 +88,9 @@ using RoME, Distributions
 using RoMEPlotting
 
 fg = initfg()
-addNode!(fg, :x0, Pose2)
+addVariable!(fg, :x0, Pose2)
 addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), eye(3))))
-addNode!(fg, :x1, Pose2)
+addVariable!(fg, :x1, Pose2)
 addFactor!(fg, [:x0;:x1], Pose2Pose2(MvNormal([10.0;0;0], eye(3))))
 
 ensureAllInitialized!(fg)

docs/src/concepts/building_graphs.md

@@ -26,10 +26,10 @@ Variables (a.k.a. poses in localization terminology) are created in the same way
 
 ```julia
 # Add the first pose :x0
-addNode!(fg, :x0, Pose2)
+addVariable!(fg, :x0, Pose2)
 # Add a few more poses
 for i in 1:10
-  addNode!(fg, Symbol("x$(i)"), Pose2)
+  addVariable!(fg, Symbol("x$(i)"), Pose2)
 end
 ```
 

docs/src/examples/basic_continuousscalar.md

@@ -27,7 +27,7 @@ The variable nodes are identified by `Symbol`s, namely `:x0, :x1, :x2, :x3`.
 fg = emptyFactorGraph()
 
 # add the first node
-addNode!(fg, :x0, ContinuousScalar)
+addVariable!(fg, :x0, ContinuousScalar)
 
 # this is unary (prior) factor and does not immediately trigger autoinit of :x0.
 addFactor!(fg, [:x0], Prior(Normal(0,1)))
@@ -60,7 +60,7 @@ Also note that initialization of variables is a local operation based only on th
 
 By adding `:x1` and connecting it through the `LinearConditional` and `Normal` distributed factor, the automatic initialization of `:x0` is triggered.
 ```julia
-addNode!(fg, :x1, ContinuousScalar)
+addVariable!(fg, :x1, ContinuousScalar)
 # P(Z | :x1 - :x0 ) where Z ~ Normal(10,1)
 addFactor!(fg, [:x0, :x1], LinearConditional(Normal(10.0,1)))
 @show isInitialized(fg, :x0) # true
@@ -105,7 +105,7 @@ The red trace (predicted belief of `:x1`) is noting more than the approximated c
 
 Another `ContinuousScalar` variable `:x2` is 'connected' to `:x1` through a more complicated `MixtureLinearConditional` likelihood function.
 ```julia
-addNode!(fg, :x2, ContinuousScalar)
+addVariable!(fg, :x2, ContinuousScalar)
 mmo = MixtureLinearConditional([Rayleigh(3); Uniform(30,55)], Categorical([0.4; 0.6]))
 addFactor!(fg, [:x1, :x2], mmo)
 ```
@@ -133,7 +133,7 @@ plotKDE(fg, [:x0, :x1, :x2])
 
 Adding one more variable `:x3` through another `LinearConditional(Normal(-50,1))`
 ```julia
-addNode!(fg, :x3, ContinuousScalar)
+addVariable!(fg, :x3, ContinuousScalar)
 addFactor!(fg, [:x2, :x3], LinearConditional(Normal(-50, 1)))
 ```
 expands the factor graph to to four variables and four factors.

docs/src/examples/basic_hexagonal2d.md

@@ -19,7 +19,7 @@ After loading the RoME and Distributions modules, we construct a local factor gr
 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(LinearAlgebra.I,3,3))) )
@@ -37,7 +37,7 @@ The next 6 nodes are added with odometry in an counter-clockwise hexagonal manne
 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
@@ -86,7 +86,7 @@ Suppose some sensor detected a feature of interest with an associated range and
 The new variable and measurement can be included into the factor graph as follows:
 ```julia
 # 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)
 

docs/src/examples/basic_slamedonut.md

@@ -68,12 +68,12 @@ Next construct the factor graph containing the first pose `:l100` (without any k
 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(LinearAlgebra.I,2,2))) )
@@ -163,7 +163,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 = Point2Point2Range( Normal(rho, 3.0) )
     addFactor!(fgl, [prev_sym;pos_sym], ppr)
@@ -178,7 +178,7 @@ function vehicle_drives_to!(fgl::FactorGraph, pos_sym::Symbol, GTp::Dict, GTl::D
       ppr = Point2Point2Range( Normal(rho, 3.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

docs/src/examples/interm_dynpose.md

@@ -12,7 +12,7 @@ A smaller example in two dimensions where we wish to estimate the velocity of so
 
 Variable nodes retain meta data (so called "soft types") describing the type of variable.  Common VariableNode types are `RoME.Point2D`, `RoME.Pose3D`.  VariableNode soft types are passed during construction of the factor graph, for example:
 ```julia
-v1 = addNode!(fg, :x1, Pose2)
+v1 = addVariable!(fg, :x1, Pose2)
 ```
 
 Certain cases require that more information be retained for each VariableNode, and velocity calculations are a clear example where time stamp data across positions is required.  
@@ -105,14 +105,14 @@ A brief usage example looks as follows, and further questions about how the prei
 ```julia
 using RoME, Distributions
 fg = initfg()
-v0 = addNode!(fg, :x0, DynPoint2(ut=0))
+v0 = addVariable!(fg, :x0, DynPoint2(ut=0))
 
 # Prior factor as boundary condition
 pp0 = DynPoint2VelocityPrior(MvNormal([zeros(2);10*ones(2)], 0.1*eye(4)))
 f0 = addFactor!(fg, [:x0;], pp0)
 
 # conditional likelihood between Dynamic Point2
-v1 = addNode!(fg, :x1, DynPoint2(ut=1000_000)) # time in microseconds
+v1 = addVariable!(fg, :x1, DynPoint2(ut=1000_000)) # time in microseconds
 dp2dp2 = DynPoint2DynPoint2(MvNormal([10*ones(2);zeros(2)], 0.1*eye(4)))
 f1 = addFactor!(fg, [:x0;:x1], dp2dp2)
 
@@ -163,9 +163,9 @@ A similar usage example here shows:
 fg = initfg()
 
 # add three point locations
-v0 = addNode!(fg, :x0, DynPoint2(ut=0))
-v1 = addNode!(fg, :x1, DynPoint2(ut=1000_000))
-v2 = addNode!(fg, :x2, DynPoint2(ut=2000_000))
+v0 = addVariable!(fg, :x0, DynPoint2(ut=0))
+v1 = addVariable!(fg, :x1, DynPoint2(ut=1000_000))
+v2 = addVariable!(fg, :x2, DynPoint2(ut=2000_000))
 
 # Prior factor as boundary condition
 pp0 = DynPoint2VelocityPrior(MvNormal([zeros(2);10*ones(2)], 0.1*eye(4)))

docs/src/examples/interm_fixedlag_hexagonal.md

@@ -22,21 +22,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/apriltagserver/todoslam.jl

@@ -9,7 +9,7 @@ for p in poses
     lmsyms = ls(fg)[2]
     atsym = Symbol("l$(detection.id)")
     if !(atsym in lmsyms)
-      addNode!(fg, atsym, Point2(labels=["LANDMARK"; "APRILTAG"]))
+      addVariable!(fg, atsym, Point2(labels=["LANDMARK"; "APRILTAG"]))
     end
 
     # reverse homography to range, bearing

examples/database/0_HexagonalSlam2d.jl

@@ -24,7 +24,7 @@ fg = Caesar.initfg(sessionname=user_config["session"], robotname=addrdict["robot
 # fg.cg = backend_config
 
 # 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], PriorPose2(MvNormal(zeros(3), 0.01*eye(3))) )
 
 # ls(fg, :x0)
@@ -38,7 +38,7 @@ addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*eye(3))) )
 for i in 0:5
   psym = Symbol("x$i")
   nsym = Symbol("x$(i+1)")
-  addNode!(fg, nsym, Pose2) # , labels=["VARIABLE";"POSE"]
+  addVariable!(fg, nsym, Pose2) # , labels=["VARIABLE";"POSE"]
   addFactor!(fg, [psym;nsym], Pose2Pose2(reshape([10.0;0;pi/3],3,1), 0.01*eye(3), [1.0]), autoinit=true )
   # Pose2Pose2_NEW(MvNormal([10.0;0;pi/3], diagm([0.1;0.1;0.1].^2)))
 end

examples/database/dbg_packedtype_conversions.jl

@@ -20,7 +20,7 @@ fg = Caesar.initfg(sessionname=user_config["session"], robotname=addrdict["robot
 
 
 # first pose :x0
-addNode!(fg, :x0, Pose2)
+addVariable!(fg, :x0, Pose2)
 
 # also add a PriorPose2 to pin the first pose at a fixed location
 addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*eye(3))) )
@@ -33,11 +33,11 @@ p1.attributes["data"]
 
 
 
-addNode!(fg, :x1, Pose2)
+addVariable!(fg, :x1, Pose2)
 addFactor!(fg, [:x0;:x1], Pose2Pose2(reshape([10.0;0;pi/3],3,1), 0.01*eye(3), [1.0]), autoinit=true )
 
 
-addNode!(fg, :x2, Pose2)
+addVariable!(fg, :x2, Pose2)
 addFactor!(fg, [:x1;:x2], Pose2Pose2(reshape([10.0;0;pi/3],3,1), 0.01*eye(3), [1.0]), autoinit=true )
 
 
@@ -56,7 +56,7 @@ inferOverTreeR!(fg, tree)
 #                           DistributionRequest("Normal", Float64[0; 0.1]),
 #                           DistributionRequest("Normal", Float64[20; 1.0]))
 
-addNode!(fg, :l1, Point2, labels=["LANDMARK";])
+addVariable!(fg, :l1, Point2, labels=["LANDMARK";])
 
 br = Pose2Point2BearingRange(Normal(0, 0.1), Normal(20, 1.0))
 

examples/database/dev/BasicCloudGraphsSLAM.jl

@@ -29,7 +29,7 @@ odoCov = diagm([3.0;3.0;0.01].^2)
 
 
 # Some starting position
-v1 = addNode!(fg, :x1, zeros(3,1), diagm([1.0;1.0;0.1]), N=N,labels=["POSE"])
+v1 = addVariable!(fg, :x1, zeros(3,1), diagm([1.0;1.0;0.1]), N=N,labels=["POSE"])
 initPosePrior = PriorPose2(MvNormal(zeros(3), initCov) )
 f1  = addFactor!(fg,[v1], initPosePrior)
 
@@ -75,14 +75,14 @@ inferOverTreeR!(fgs, tree)
 # cov = [3.0]
 #
 # # robot style, add first pose vertex
-# v1 = addNode!(fg,:x1,doors,N=N)
+# v1 = addVariable!(fg,:x1,doors,N=N)
 #
 # # add a prior for the initial position of the robot
 # f0  = addFactor!(fg,[v1], Obsv2(doors, cov', [1.0]))
 #
 # # add second pose vertex
 # tem = 2.0*randn(1,N)+getVal(v1)+50.0
-# v2 = addNode!(fg, :x2, tem, N=N)
+# v2 = addVariable!(fg, :x2, tem, N=N)
 #
 # # now add the odometry factor between them
 # f1 = addFactor!(fg,[v1;v2],Odo([50.0]',[2.0]',[1.0]))

examples/database/dev/BuildCloudFactorGraph.jl

@@ -25,19 +25,19 @@ doors = [-100.0;0.0;100.0;300.0]'
 cov = [3.0]
 
 # robot style, add first pose vertex
-v1 = addNode!(fg,:x1,doors,N=N,labels=["POSE"])
+v1 = addVariable!(fg,:x1,doors,N=N,labels=["POSE"])
 
 # add a prior for the initial position of the robot
 f0  = addFactor!(fg,[v1], Obsv2(doors, cov', [1.0]))
 
 # add second pose vertex
 tem = 2.0*randn(1,N)+getVal(v1)+50.0
-v2 = addNode!(fg, :x2, tem, N=N,labels=["POSE"])
+v2 = addVariable!(fg, :x2, tem, N=N,labels=["POSE"])
 
 # now add the odometry factor between them
 f1 = addFactor!(fg,[v1;v2],Odo([50.0]',[2.0]',[1.0]))
 
-v3=addNode!(fg,:x3,4.0*randn(1,N)+getVal(v2)+50.0, N=N,labels=["POSE"])
+v3=addVariable!(fg,:x3,4.0*randn(1,N)+getVal(v2)+50.0, N=N,labels=["POSE"])
 addFactor!(fg,[v2;v3],Odo([50.0]',[4.0]',[1.0]))
 f2 = addFactor!(fg,[v3], Obsv2(doors, cov', [1.0]))
 
@@ -58,25 +58,25 @@ println("Waiting for initial solve to occur")
 sleep(10)
 
 
-v4=addNode!(fg,:x4,2.0*randn(1,N)+getVal(v3)+50.0, N=N,labels=["POSE"])
+v4=addVariable!(fg,:x4,2.0*randn(1,N)+getVal(v3)+50.0, N=N,labels=["POSE"])
 addFactor!(fg,[v3;v4],Odo([50.0]',[2.0]',[1.0]))
 
 if true
-    l1=addNode!(fg, :l1, 0.5*randn(1,N)+getVal(v3)+64.0, N=N,labels=["LANDMARK"])
+    l1=addVariable!(fg, :l1, 0.5*randn(1,N)+getVal(v3)+64.0, N=N,labels=["LANDMARK"])
     addFactor!(fg, [v3,l1], Ranged([64.0],[0.5],[1.0]))
     addFactor!(fg, [v4,l1], Ranged([16.0],[0.5],[1.0]))
 end
 
 
-v5=addNode!(fg,:x5,2.0*randn(1,N)+getVal(v4)+50.0, N=N,labels=["POSE"])
+v5=addVariable!(fg,:x5,2.0*randn(1,N)+getVal(v4)+50.0, N=N,labels=["POSE"])
 addFactor!(fg,[v4;v5],Odo([50.0]',[2.0]',[1.0]))
 
 
-v6=addNode!(fg,:x6,1.25*randn(1,N)+getVal(v5)+40.0, N=N,labels=["POSE"])
+v6=addVariable!(fg,:x6,1.25*randn(1,N)+getVal(v5)+40.0, N=N,labels=["POSE"])
 addFactor!(fg,[v5;v6],Odo([40.0]',[1.25]',[1.0]))
 
 
-v7=addNode!(fg,:x7,2.0*randn(1,N)+getVal(v6) +60.0, N=N,labels=["POSE"])
+v7=addVariable!(fg,:x7,2.0*randn(1,N)+getVal(v6) +60.0, N=N,labels=["POSE"])
 addFactor!(fg,[v6;v7],Odo([60.0]',[2.0]',[1.0]))
 
 f3 = addFactor!(fg,[v7], Obsv2(doors, cov', [1.0]))

examples/dev/FreezeVariables.jl

@@ -6,18 +6,18 @@ sessionId = "Test"
 fg = Caesar.initfg(sessionname=sessionId, robotname=robotId)
 
 # 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], PriorPose2(MvNormal(zeros(3), 0.01*Matrix(LinearAlgebra.I, 3,3))) )
 
 
 # Drive around in a hexagon
 for i in 0:5
   psym = Symbol("x$i")
   nsym = Symbol("x$(i+1)")
-  addNode!(fg, nsym, Pose2) # , labels=["VARIABLE";"POSE"]
+  addVariable!(fg, nsym, Pose2) # , labels=["VARIABLE";"POSE"]
   addFactor!(fg, [psym;nsym], Pose2Pose2( MvNormal([10.0;0;pi/3], 0.01*Matrix(LinearAlgebra.I, 3,3)) ), autoinit=true )
   # Pose2Pose2_NEW(MvNormal([10.0;0;pi/3], diagm([0.1;0.1;0.1].^2)))
 end
 
 
-response = addNode!(fg, :l1, Point2("LANDMARK"))
+response = addVariable!(fg, :l1, Point2("LANDMARK"))

examples/dfg/HexagonalDFG_PoC.jl

@@ -40,7 +40,7 @@ addF!(dfg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*eye(3))) )
 for i in 0:5
   psym = Symbol("x$i")
   nsym = Symbol("x$(i+1)")
-  addNode!(fg, nsym, Pose2) # , labels=["VARIABLE";"POSE"]
+  addVariable!(fg, nsym, Pose2) # , labels=["VARIABLE";"POSE"]
   addFactor!(fg, [psym;nsym], Pose2Pose2(reshape([10.0;0;pi/3],3,1), 0.01*eye(3), [1.0]), autoinit=true )
   # Pose2Pose2_NEW(MvNormal([10.0;0;pi/3], diagm([0.1;0.1;0.1].^2)))
 en

examples/marine/asv/kayaks/SAS-SLAM.jl

@@ -38,7 +38,7 @@ saveBF = Dict();
 savedirheader = "0813/0813";
 
 fg = initfg()
-addNode!(fg, beacon, Point2)
+addVariable!(fg, beacon, Point2)
 
 for fitr in 1:nfactors
     navchecked = false;