Merge pull request #155 from JuliaRobotics/maintenance/prep031

updates and fixes
JuliaRobotics · Feb 15, 2019 · 7eaacb9 · 7eaacb9
2 parents 16fccf6 + 9c0d330
commit 7eaacb9
Show file tree

Hide file tree

Showing 11 changed files with 270 additions and 242 deletions.
diff --git a/examples/KinematicJoints.jl b/examples/KinematicJoints.jl
@@ -66,14 +66,14 @@ fg = RoME.initfg()
 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)
+initVariable!(fg, :x1)
 
 
 # torso
 addVariable!(fg, :x2, dims=6)
 hip = ZJoint(Normal(pi/3,0.1))
 addFactor!(fg, [:x1, :x2], hip) # hio
-initializeNode!(fg, :x2)
+initVariable!(fg, :x2)
 
 
 
@@ -114,7 +114,7 @@ plotPose3Pairs(fg, :x2)
 addVariable!(fg, :x3, dims=6)
 should = XJoint(Normal(pi/4,0.1))
 addFactor!(fg, [:x2, :x3], should) # hio
-initializeNode!(fg, :x3)
+initVariable!(fg, :x3)
 
 
 
@@ -132,7 +132,7 @@ solveandvisualize(fg, vis)
 addVariable!(fg, :x4, dims=6)
 should = XJoint(Normal(pi/4,0.1))
 addFactor!(fg, [:x3, :x4], should) # hio
-initializeNode!(fg, :x4)
+initVariable!(fg, :x4)
 
 
 
@@ -159,19 +159,19 @@ fg = RoME.initfg()
 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)
+initVariable!(fg, :x1)
 
 
 addVariable!(fg, :x2, dims=6)
 hip = ZJoint(Uniform(-pi/3,pi/3))
 addFactor!(fg, [:x1, :x2], hip) # hio
-initializeNode!(fg, :x2)
+initVariable!(fg, :x2)
 
 
 addVariable!(fg, :x3, dims=6)
 should = XJoint(Uniform(-pi/4,pi/4))
 addFactor!(fg, [:x2, :x3], should) # hio
-initializeNode!(fg, :x3)
+initVariable!(fg, :x3)
 
 
 visualizeallposes!(vis, fg, drawtype=:max)

diff --git a/examples/RangesExample.jl b/examples/RangesExample.jl
@@ -48,17 +48,17 @@ addFactor!(fg, [:l2;], PriorPoint2(MvNormal(GTl[:l2], Matrix{Float64}(LinearAlge
 
 # first range measurement
 rhoZ1 = norm(GTl[:l1]-GTp[:l100])
-ppr = Point2DPoint2DRange([rhoZ1], 2.0, [1.0])
+ppr = Point2Point2Range(Normal(rhoZ1, 2.0))
 addFactor!(fg, [:l100;:l1], ppr)
 
 # second range measurement
 rhoZ2 = norm(GTl[:l2]-GTp[:l100])
-ppr = Point2DPoint2DRange([rhoZ2], 3.0, [1.0])
+ppr = Point2Point2Range(Normal(rhoZ2, 3.0))
 addFactor!(fg, [:l100; :l2], ppr)
 
 # second range measurement
 rhoZ3 = norm(GTl[:l3]-GTp[:l100])
-ppr = Point2DPoint2DRange([rhoZ3], 3.0, [1.0])
+ppr = Point2Point2Range(Normal(rhoZ3, 3.0))
 addFactor!(fg, [:l100; :l3], ppr)
 
 
@@ -100,7 +100,7 @@ function vehicle_drives_to!(fgl::FactorGraph, pos_sym::Symbol, GTp::Dict, GTl::D
     println("Adding variable vertex $pos_sym, not yet in fgl::FactorGraph.")
     addVariable!(fgl, pos_sym, Point2)
     @show rho = norm(GTp[prev_sym] - GTp[pos_sym])
-    ppr = Point2DPoint2DRange([rho], 3.0, [1.0])
+    ppr = Point2Point2Range(Normal(rho, 3.0))
     addFactor!(fgl, [prev_sym;pos_sym], ppr)
   else
     @warn "Variable node $pos_sym already in the factor graph."
@@ -110,7 +110,7 @@ function vehicle_drives_to!(fgl::FactorGraph, pos_sym::Symbol, GTp::Dict, GTl::D
     rho = norm(GTl[ll] - GTp[pos_sym])
     # Check for feasible measurements:  vehicle within 150 units from the beacons/landmarks
     if rho < measurelimit
-      ppr = Point2DPoint2DRange([rho], 3.0, [1.0])
+      ppr = Point2Point2Range(Normal(rho, 3.0))
       if !(ll in currvar)
         println("Adding variable vertex $ll, not yet in fgl::FactorGraph.")
         addVariable!(fgl, ll, Point2)