Merge pull request #153 from JuliaRobotics/maintenance/addmanifoldsinfo

Maintenance/addmanifoldsinfo

REQUIRE

@@ -1,10 +1,11 @@
 julia 0.7 1.1
-IncrementalInference 0.4.6 0.5.0
+IncrementalInference 0.5.0
 Graphs 0.10.1
 TransformUtils 0.2.2
 CoordinateTransformations 0.5.0
 Rotations 0.9.1
-KernelDensityEstimate 0.4.1 0.4.3
+KernelDensityEstimate 0.4.3
+ApproxManifoldProducts 0.0.3
 Distributions 0.16.4
 JLD2 0.1.2
 FileIO 1.0.2

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/DynPoint2D.jl

@@ -9,7 +9,8 @@ mutable struct DynPoint2 <: IncrementalInference.InferenceVariable
   ut::Int64 # microsecond time
   dims::Int
   labels::Vector{String}
-  DynPoint2(;ut::Int64=-9999999999, labels::Vector{<:AbstractString}=String[]) = new(ut, 4, labels)
+  manifolds::Tuple{Symbol, Symbol, Symbol, Symbol}
+  DynPoint2(;ut::Int64=-9999999999, labels::Vector{<:AbstractString}=String[]) = new(ut, 4, labels,(:Euclid,:Euclid,:Euclid,:Euclid,))
 end
 
 

src/DynPose2D.jl

@@ -25,7 +25,8 @@ mutable struct DynPose2 <: IncrementalInference.InferenceVariable
   ut::Int64 # microsecond time
   dims::Int
   labels::Vector{String}
-  DynPose2(;ut::Int64=-9999999999, labels::Vector{<:AbstractString}=String["POSE";]) = new(ut, 5, labels)
+  manifolds::Tuple{Symbol,Symbol,Symbol,Symbol,Symbol}
+  DynPose2(;ut::Int64=-9999999999, labels::Vector{<:AbstractString}=String[]) = new(ut, 5, labels, (:Euclid,:Euclid,:Circular,:Euclid,:Euclid))
 end
 
 

src/Point2D.jl

@@ -5,7 +5,8 @@ $(TYPEDEF)
 struct Point2 <: IncrementalInference.InferenceVariable
   dims::Int
   labels::Vector{String}
-  Point2() = new(2, String[])
+  manifolds::Tuple{Symbol,Symbol}
+  Point2(;labels::Vector{<:AbstractString}=String[]) = new(2, labels, (:Euclid, :Euclid))
 end
 
 """
@@ -143,7 +144,7 @@ mutable struct PackedPriorPoint2DensityNH <: IncrementalInference.PackedInferenc
 end
 function convert(::Type{PriorPoint2DensityNH}, d::PackedPriorPoint2DensityNH)
   return PriorPoint2DensityNH(
-            kde!(EasyMessage( reshapeVec2Mat(d.rpts, d.dims), d.rbw)),
+            AMP.kde!(EasyMessage( reshapeVec2Mat(d.rpts, d.dims), d.rbw, (:Euclid, :Euclid))),
             Distributions.Categorical(d.nh)  )
 end
 function convert(::Type{PackedPriorPoint2DensityNH}, d::PriorPoint2DensityNH)

src/Pose2D.jl

@@ -24,7 +24,8 @@ $(TYPEDEF)
 struct Pose2 <: IncrementalInference.InferenceVariable
   dims::Int
   labels::Vector{String}
-  Pose2() = new(3, String["POSE";])
+  manifolds::Tuple{Symbol,Symbol,Symbol}
+  Pose2(;labels::Vector{<:AbstractString}=String[]) = new(3, labels, (:Euclid, :Euclid, :Circular))
 end
 
 # # Done - move to IncrementalInference

src/Pose3D.jl

@@ -4,7 +4,8 @@ $(TYPEDEF)
 struct Point3 <: IncrementalInference.InferenceVariable
   dims::Int
   labels::Vector{String}
-  Point3() = new(3, String[])
+  manifolds::Tuple{Symbol, Symbol, Symbol}
+  Point3() = new(3, String[], (:Euclid,:Euclid,:Euclid))
 end
 
 """
@@ -13,5 +14,7 @@ $(TYPEDEF)
 struct Pose3 <: IncrementalInference.InferenceVariable
   dims::Int
   labels::Vector{String}
-  Pose3() = new(6, String["POSE";])
+  manifolds::Tuple{Symbol,Symbol,Symbol,Symbol,Symbol,Symbol}
+  # TODO the AMP upgrade is aimed at resolving 3D to Quat/SE3/SP3 -- current Euler angles will be replaced
+  Pose3() = new(6, String[], (:Euclid,:Euclid,:Euclid,:Euclid,:Euclid,:Euclid) )
 end

src/RoME.jl

@@ -6,6 +6,7 @@ using Reexport
 @reexport using TransformUtils
 @reexport using Distributions
 @reexport using KernelDensityEstimate
+@reexport using ApproxManifoldProducts
 
 using
   Distributed,
@@ -22,6 +23,7 @@ import Base: +, \, convert
 import TransformUtils: ⊖, ⊕, convert, compare, ominus, veeQuaternion
 import IncrementalInference: convert, getSample, reshapeVec2Mat, extractdistribution  #, compare
 
+const AMP = ApproxManifoldProducts
 
 export
   initfg,  # RoME specific functions