Merge pull request #42 from dehann/feature/addslam2dautoinittutorial

Feature/addslam2dautoinittutorial

examples/SeparateInitializationProcess.jl

@@ -0,0 +1,119 @@
+# tutorial on conventional 2D SLAM example
+
+addprocs(3)
+
+# This tutorial shows how to use some of the commonly used factor types
+# This tutorial follows from the ContinuousScalar example from IncrementalInference
+
+@everywhere begin
+using RoME, Distributions
+using IncrementalInference
+using TransformUtils
+
+import IncrementalInference: getSample
+
+# struct Pose2 <: IncrementalInference.InferenceVariable
+#   dims::Int
+#   Pose2() = new(3)
+# end
+# struct Point2 <: IncrementalInference.InferenceVariable
+#   dims::Int
+#   Point2() = new(2)
+# end
+
+
+struct Prior{T} <: IncrementalInference.FunctorSingleton where {T <: Distribution}
+  z::T
+end
+getSample(s::Prior, N::Int=1) = (rand(s.z,N), )
+
+struct Pose2Pose2_NEW{T} <: IncrementalInference.FunctorPairwise where {T <: Distribution}
+  z::T
+  Pose2Pose2_NEW() where {T <: Distribution} = new{T}()
+  Pose2Pose2_NEW(z1::T) where {T <: Distribution} = new{T}(z1)
+  Pose2Pose2_NEW(mean::Vector{Float64}, cov::Array{Float64,2}) where {T <: Distribution} = new{Distributions.MvNormal}(MvNormal(mean, cov))
+end
+getSample(s::Pose2Pose2_NEW, N::Int=1) = (rand(s.z,N), )
+function (s::Pose2Pose2_NEW)(res::Array{Float64},
+      idx::Int,
+      meas::Tuple,
+      wxi::Array{Float64,2},
+      wxj::Array{Float64,2}  )
+  # res[1] = meas[1][idx] - (X2[1,idx] - X1[1,idx])
+  wXjhat = SE2(wxi[:,idx])*SE2(meas[1][:,idx]) #*SE2(pp2.Zij[:,1])*SE2(meas[1][:,idx])
+  jXjhat = SE2(wxj[:,idx]) \ wXjhat
+  se2vee!(res, jXjhat)
+  nothing
+end
+end # everywhere
+
+
+# Start with an empty graph
+fg = initfg(sessionname="SLAM2D_TUTORIAL")
+
+
+# also add a PriorPose2 to pin the first pose at a fixed location
+addNode!(fg, :x0, Pose2, labels=["POSE"])
+addFactor!(fg, [:x0], Prior(MvNormal([0.0;0.0;0], diagm([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, labels=["POSE"])
+  addFactor!(fg, [psym;ensureAllInitialized!nsym], Pose2Pose2_NEW(MvNormal([10.0;0;pi/3], diagm([0.1;0.1;0.1].^2))))
+end
+
+# Graphs.plot(fg.g)
+
+# Add landmarks with Bearing range measurements
+addNode!(fg, :l1, Point2, labels=["LANDMARK"])
+p2br = Pose2DPoint2DBearingRange(Normal(0,0.1),Normal(20.0,1.0))
+addFactor!(fg, [:x0; :l1], p2br)
+
+
+# Add landmarks with Bearing range measurements
+p2br2 = Pose2DPoint2DBearingRange(Normal(0,0.1),Normal(20.0,1.0))
+addFactor!(fg, [:x6; :l1], p2br2)
+
+
+isInitialized(fg, :l1)
+
+
+# Replace this call with an initialization service that traverses the entire graph
+ensureAllInitialized!(fg)
+
+
+
+# get all initialized nodes
+
+
+
+
+
+using RoMEPlotting, Gadfly
+
+
+
+pl = plotKDE(fg, [:x0; :x1; :x2; :x3; :x4; :x5; :x6]);
+
+Gadfly.draw(PDF("tmpX0123456.pdf", 15cm, 15cm), pl)
+
+@async run(`evince tmpX0123456.pdf`)
+
+
+
+# pl = drawPoses(fg)
+pl = drawPosesLandms(fg)
+Gadfly.draw(PDF("tmpPosesFg.pdf", 15cm, 15cm), pl)
+@async run(`evince tmpPosesFg.pdf`)
+
+
+
+# Perform inference over the entire graph
+
+tree = wipeBuildNewTree!(fg)
+# @async Graphs.plot(tree.bt)
+@time inferOverTree!(fg, tree)
+
+#

examples/Slam2dExample.jl

@@ -0,0 +1,109 @@
+# tutorial on conventional 2D SLAM example
+
+addprocs(3)
+
+# This tutorial shows how to use some of the commonly used factor types
+# This tutorial follows from the ContinuousScalar example from IncrementalInference
+
+@everywhere begin
+  using RoME, Distributions
+  using IncrementalInference
+  using TransformUtils
+end # everywhere
+
+
+# Start with an empty graph
+fg = initfg(sessionname="SLAM2D_TUTORIAL")
+
+
+# also add a PriorPose2 to pin the first pose at a fixed location
+addNode!(fg, :x0, Pose2, labels=["POSE"])
+addFactor!(fg, [:x0], Prior(MvNormal([0.0;0.0;0], diagm([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, labels=["POSE"])
+  addFactor!(fg, [psym;nsym], Pose2Pose2_NEW(MvNormal([10.0;0;pi/3], diagm([0.1;0.1;0.1].^2))))
+end
+
+# Graphs.plot(fg.g)
+
+
+# Add landmarks with Bearing range measurements
+addNode!(fg, :l1, Point2, labels=["LANDMARK"])
+p2br = Pose2DPoint2DBearingRange(Normal(0,0.1),Normal(20.0,1.0))
+addFactor!(fg, [:x0; :l1], p2br)
+
+
+# Add landmarks with Bearing range measurements
+p2br2 = Pose2DPoint2DBearingRange(Normal(0,0.1),Normal(20.0,1.0))
+addFactor!(fg, [:x6; :l1], p2br2)
+
+
+
+isInitialized(fg, :l1)
+
+
+ensureAllInitialized!(fg)
+
+
+
+
+using RoMEPlotting, Gadfly
+
+
+
+
+
+pl = plotKDE(fg, [:x0; :x1; :x2; :x3; :x4; :x5; :x6]);
+
+Gadfly.draw(PDF("tmpX0123456.pdf", 15cm, 15cm), pl)
+
+@async run(`evince tmpX0123456.pdf`)
+
+
+
+# pl = drawPoses(fg)
+pl = drawPosesLandms(fg)
+Gadfly.draw(PDF("tmpPosesFg.pdf", 15cm, 15cm), pl)
+@async run(`evince tmpPosesFg.pdf`)
+
+
+
+tree = wipeBuildNewTree!(fg)
+
+
+@async Graphs.plot(tree.bt)
+
+
+@time inferOverTree!(fg, tree)
+
+
+# Graphs.plot(tree.bt)
+
+@async Graphs.plot(fg.g)
+
+
+# These functions need more work
+
+using KernelDensityEstimate
+
+function plot(::Type{Pose2}, p::Vector{BallTreeDensity})
+
+  xy = plotKDE(p,dims=[1;2])
+  th = plotKDE(p,dims=[3])
+
+  hstack(xy, th)
+end
+
+vert0 = getVert(fg, :x0)
+vert1 = getVert(fg, :x1)
+pl = plot(typeof(getData(vert).softtype), [getKDE(vert0); getKDE(vert1)])
+
+Gadfly.draw(PDF("tmpX01.pdf", 15cm, 15cm), pl)
+@async run(`evince tmpX01.pdf`)
+
+
+#

src/BearingRange2D.jl

@@ -40,6 +40,7 @@ type Pose2DPoint2DBearingRange{B <: Distributions.Distribution, R <: Distributio
     bearing::B
     range::R
     Pose2DPoint2DBearingRange{B,R}() where {B,R} = new{B,R}()
+    Pose2DPoint2DBearingRange(x1::B,x2::R) where {B,R} = new{B,R}(x1,x2)
     Pose2DPoint2DBearingRange{B,R}(x1::B,x2::R) where {B,R} = new{B,R}(x1,x2)
 end
 function getSample(pp2br::Pose2DPoint2DBearingRange, N::Int=1)
@@ -89,6 +90,50 @@ end
 
 
 
+
+
+
+
+
+#-------------------------------------------------------------------------------
+# bearing only available
+
+# this factor type is still a work in progress
+type Pose2DPoint2DBearing{B <: Distributions.Distribution} <: IncrementalInference.FunctorPairwise
+    bearing::B
+    Pose2DPoint2DBearing{B}() where {B} = new{B}()
+    Pose2DPoint2DBearing(x1::B) where {B} = new{B}(x1)
+    Pose2DPoint2DBearing{B}(x1::B) where {B} = new{B}(x1)
+end
+function getSample(pp2br::Pose2DPoint2DBearing, N::Int=1)
+  return (rand(pp2br.bearing, N), )
+end
+# define the conditional probability constraint
+function (pp2br::Pose2DPoint2DBearing)(res::Array{Float64},
+        idx::Int,
+        meas::Tuple,
+        xi::Array{Float64,2},
+        lm::Array{Float64,2} )
+  #
+  res[1] = meas[1][idx] - atan2(lm[2,idx]-xi[2,idx], lm[1,idx]-xi[1,idx])
+  nothing
+end
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 # to be deprecated
 function pack3(xL1, xL2, p1, p2, p3, xF3)
     warn("RoME.BearingRange2D:pack3 to be deprecated")

src/Pose2D.jl

@@ -1,5 +1,42 @@
 # Pose2 functions for Robot Motion Estimate
 
+# TODO - move to IncrementalInference
+struct Prior{T} <: IncrementalInference.FunctorSingleton where {T <: Distribution}
+  z::T
+end
+getSample(s::Prior, N::Int=1) = (rand(s.z,N), )
+
+
+struct Pose2 <: IncrementalInference.InferenceVariable
+  dims::Int
+  Pose2() = new(3)
+end
+struct Point2 <: IncrementalInference.InferenceVariable
+  dims::Int
+  Point2() = new(2)
+end
+
+
+struct Pose2Pose2_NEW{T} <: IncrementalInference.FunctorPairwise where {T <: Distribution}
+  z::T
+  Pose2Pose2_NEW() where {T <: Distribution} = new{T}()
+  Pose2Pose2_NEW(z1::T) where {T <: Distribution} = new{T}(z1)
+  Pose2Pose2_NEW(mean::Vector{Float64}, cov::Array{Float64,2}) where {T <: Distribution} = new{Distributions.MvNormal}(MvNormal(mean, cov))
+end
+getSample(s::Pose2Pose2_NEW, N::Int=1) = (rand(s.z,N), )
+function (s::Pose2Pose2_NEW)(res::Array{Float64},
+      idx::Int,
+      meas::Tuple,
+      wxi::Array{Float64,2},
+      wxj::Array{Float64,2}  )
+  # res[1] = meas[1][idx] - (X2[1,idx] - X1[1,idx])
+  wXjhat = SE2(wxi[:,idx])*SE2(meas[1][:,idx]) #*SE2(pp2.Zij[:,1])*SE2(meas[1][:,idx])
+  jXjhat = SE2(wxj[:,idx]) \ wXjhat
+  se2vee!(res, jXjhat)
+  nothing
+end
+
+
 
 type PriorPose2 <: IncrementalInference.FunctorSingleton
     Zi::Array{Float64,2}

src/RoME.jl

@@ -64,8 +64,8 @@ export
   loadjld,
   FactorGraph,
   initializeNode!,
-  plotKDE,
-  plot, # for Graphs.plot
+  isInitialized,
+  ensureAllInitialized!,
   # overloaded functions from IIF
   # decodefg,
   # convertfrompackedfunctionnode,
@@ -179,6 +179,7 @@ export
   PackedPoint2DPoint2D,
   Pose2DPoint2DBearingRange,
   PackedPose2DPoint2DBearingRange,
+  Pose2DPoint2DBearing,
   Pose2DPoint2DRange,
   Point2DPoint2DRange,
   PackedPoint2DPoint2DRange,
@@ -199,6 +200,11 @@ export
   Pose2DPoint2DRangeDensity,
   PackedPose2DPoint2DRangeDensity,
 
+  Pose2,
+  Point2,
+  Prior,
+  Pose2Pose2_NEW,
+
   # Pose2D
   PriorPose2,
   PackedPriorPose2,

src/RobotUtils.jl

@@ -177,14 +177,14 @@ function addOdoFG!(
       N = c
     end
     sig = diag(cov)
-    RES = zeros(r,c)
+    XnextInit = zeros(r,c)
     # increases the number of particles based on the number of modes in the measurement Z
     for i in 1:c
         ent = [randn()*sig[1]; randn()*sig[2]; randn()*sig[3]]
-        RES[:,i] = addPose2Pose2(X[:,i], DX + ent)
+        XnextInit[:,i] = addPose2Pose2(X[:,i], DX + ent)
     end
 
-    v = addNode!(fg, n, RES, cov, N=N, ready=ready, labels=labels)
+    v = addNode!(fg, n, XnextInit, cov, N=N, ready=ready, labels=labels)
     pp = Pose2Pose2(vectoarr2(DX), cov, [1.0]) #[prev;v],
     f = addFactor!(fg, [prev;v], pp, ready=ready)
     infor = inv(cov^2)