Merge pull request #238 from JuliaRobotics/fix/1Q20/iif580

DFG v0.6 and remove labels

Project.toml

@@ -2,7 +2,7 @@ name = "RoME"
 uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 keywords = ["SLAM", "state-estimation", "mm-iSAM", "inference", "robotics"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.5.5"
+version = "0.6.0"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -28,12 +28,12 @@ TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 [compat]
 ApproxManifoldProducts = "0.1, 0.2"
 CoordinateTransformations = "0.5, 0.6, 0.7"
-DistributedFactorGraphs = "^0.5.1"
+DistributedFactorGraphs = "0.6.2"
 Distributions = "0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 0.24, 0.25, 1"
 DocStringExtensions = "0.7, 0.8, 0.9, 0.10, 1"
 FileIO = "1.0.2, 1.1, 1.2"
 Graphs = "0.10.2, 0.11, 1"
-IncrementalInference = "^0.8.2"
+IncrementalInference = "0.9"
 JLD2 = "0.1, 0.2, 0.3, 1.0"
 KernelDensityEstimate = "0.5.1, 0.6"
 Optim = "0.16, 0.17, 0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 1"

src/CanonicalGraphs.jl

@@ -81,6 +81,38 @@ function generateCanonicalFG_Circle(poses::Int=6;
   return fg
 end
 
+
+function driveHex(fgl, posecount::Int; steps::Int=5)
+    # Drive around in a hexagon
+    for i in (posecount-1):(posecount-1+steps)
+        psym = Symbol("x$i")
+        posecount += 1
+        nsym = Symbol("x$(i+1)")
+        addVariable!(fgl, nsym, Pose2)
+        pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
+        addFactor!(fgl, [psym;nsym], pp, autoinit=false )
+    end
+
+    return posecount
+end
+
+
+function offsetHexLeg(dfg::G, posecount::Int; direction=:right) where G <: AbstractDFG
+    psym = Symbol("x$(posecount-1)")
+    nsym = Symbol("x$(posecount)")
+    posecount += 1
+    addVariable!(dfg, nsym, Pose2)
+    pp = nothing
+    if direction == :right
+        pp = Pose2Pose2(MvNormal([10.0;0;-pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
+    elseif direction == :left
+        pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
+    end
+    addFactor!(dfg, [psym; nsym], pp, autoinit=false )
+    return posecount
+end
+
+
 """
     $SIGNATURES
 

src/RoME.jl

@@ -23,6 +23,7 @@ using
 import Base: +, \, convert
 import TransformUtils: ⊖, ⊕, convert, compare, ominus, veeQuaternion
 import IncrementalInference: convert, getSample, reshapeVec2Mat, extractdistribution, DFG
+# not sure why this is gives import error
 import DistributedFactorGraphs: compare
 
 # const AMP = ApproxManifoldProducts

src/factors/BearingRange2D.jl

@@ -29,8 +29,8 @@ function (pp2br::Pose2Point2BearingRange)(res::Array{Float64},
                                           xi::Array{Float64,2},
                                           lm::Array{Float64,2} )
   #
-
   rot = meas[1][1,idx]+xi[3,idx]
+
   res[1] = ( lm[1,idx] - (meas[1][2,idx]*cos( rot ) + xi[1,idx]) )^2
   res[2] = ( lm[2,idx] - (meas[1][2,idx]*sin( rot ) + xi[2,idx]) )^2
 

src/factors/Polar.jl

@@ -7,8 +7,7 @@ Range and theta definition on `(:Euclid, :Circular)` manifold.
 struct Polar <: IIF.InferenceVariable
   dims::Int
   manifolds::Tuple{Symbol,Symbol}
-  labels::Vector{String}
-  Polar() = new(2,(:Euclid,:Circular),String[])
+  Polar() = new(2,(:Euclid,:Circular))
 end
 
 """

src/variables/DynPoint2D.jl

@@ -8,7 +8,6 @@ Dynamic point in 2D space with velocity components: `x, y, dx/dt, dy/dt`
 mutable struct DynPoint2 <: IncrementalInference.InferenceVariable
   ut::Int64 # microsecond time
   dims::Int
-  labels::Vector{String}
   manifolds::Tuple{Symbol, Symbol, Symbol, Symbol}
-  DynPoint2(;ut::Int64=-9999999999, labels::Vector{<:AbstractString}=String[]) = new(ut, 4, labels,(:Euclid,:Euclid,:Euclid,:Euclid,))
+  DynPoint2(;ut::Int64=-9999999999) = new(ut, 4,(:Euclid,:Euclid,:Euclid,:Euclid,))
 end

src/variables/DynPose2D.jl

@@ -6,7 +6,6 @@ Dynamic pose variable with velocity components: `x, y, theta, dx/dt, dy/dt`
 mutable struct DynPose2 <: IncrementalInference.InferenceVariable
   ut::Int64 # microsecond time
   dims::Int
-  labels::Vector{String}
   manifolds::Tuple{Symbol,Symbol,Symbol,Symbol,Symbol}
-  DynPose2(;ut::Int64=-9999999999, labels::Vector{<:AbstractString}=String[]) = new(ut, 5, labels, (:Euclid,:Euclid,:Circular,:Euclid,:Euclid))
+  DynPose2(;ut::Int64=-9999999999) = new(ut, 5, (:Euclid,:Euclid,:Circular,:Euclid,:Euclid))
 end

src/variables/Point2D.jl

@@ -6,7 +6,6 @@ XY Euclidean manifold variable node softtype.
 """
 struct Point2 <: IncrementalInference.InferenceVariable
   dims::Int
-  labels::Vector{String}
   manifolds::Tuple{Symbol,Symbol}
-  Point2(;labels::Vector{<:AbstractString}=String[]) = new(2, labels, (:Euclid, :Euclid))
+  Point2() = new(2, (:Euclid, :Euclid))
 end

src/variables/Point3D.jl

@@ -12,7 +12,6 @@ p3 = Point3()
 """
 struct Point3 <: IncrementalInference.InferenceVariable
   dims::Int
-  labels::Vector{String}
   manifolds::Tuple{Symbol, Symbol, Symbol}
-  Point3() = new(3, String[], (:Euclid,:Euclid,:Euclid))
+  Point3() = new(3, (:Euclid,:Euclid,:Euclid))
 end

src/variables/Pose2D.jl

@@ -7,7 +7,6 @@ Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular ro
 """
 struct Pose2 <: IncrementalInference.InferenceVariable
   dims::Int
-  labels::Vector{String}
   manifolds::Tuple{Symbol,Symbol,Symbol}
-  Pose2(;labels::Vector{<:AbstractString}=String[]) = new(3, labels, (:Euclid, :Euclid, :Circular))
+  Pose2() = new(3, (:Euclid, :Euclid, :Circular))
 end

src/variables/Pose3D.jl

@@ -10,8 +10,7 @@ Future:
 """
 struct Pose3 <: IncrementalInference.InferenceVariable
   dims::Int
-  labels::Vector{String}
   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,:Circular,:Circular,:Circular) )
+  Pose3() = new(6, (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular) )
 end

test/BeehiveTestUtils.jl

@@ -1,31 +1 @@
 
-
-function driveHex(fgl, posecount::Int; steps::Int=5)
-    # Drive around in a hexagon
-    for i in (posecount-1):(posecount-1+steps)
-        psym = Symbol("x$i")
-        posecount += 1
-        nsym = Symbol("x$(i+1)")
-        addVariable!(fgl, nsym, Pose2)
-        pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
-        addFactor!(fgl, [psym;nsym], pp, autoinit=false )
-    end
-
-    return posecount
-end
-
-
-function offsetHexLeg(dfg::G, posecount::Int; direction=:right) where G <: AbstractDFG
-    psym = Symbol("x$(posecount-1)")
-    nsym = Symbol("x$(posecount)")
-    posecount += 1
-    addVariable!(dfg, nsym, Pose2)
-    pp = nothing
-    if direction == :right
-        pp = Pose2Pose2(MvNormal([10.0;0;-pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
-    elseif direction == :left
-        pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
-    end
-    addFactor!(dfg, [psym; nsym], pp, autoinit=false )
-    return posecount
-end

test/runtests.jl

@@ -4,90 +4,45 @@
 using RoME
 using Test
 
-# might be unnecessary
-using JLD2  #, HDF5
 
-println("[TEST] numeric root...")
-include("testhigherdimroots.jl")
-println("[SUCCESS]")
-
-include("testManifoldsPose2Equivalent.jl")
-
-# Requires standardized testing strategy
-# println("[TEST] Camera function evaluations...")
-# include("testCameraFunctions.jl")
-# println("[SUCCESS]")
-
-@warn "Skipping multiple feature constraint test for the time being"
-# println("[TEST] MultipleFeatures constraints")
-# include("testmultiplefeatures.jl")
-# println("[SUCCESS]")
-
-println("[TEST] Linear array function evaluations...")
-include("testDidsonFunctions.jl")
-println("[SUCCESS]")
-
-include("testPoint2Point2.jl")
-
-include("testPoint2Point2Init.jl")
-
-include("testBasicPose2Stationary.jl")
-
-println("[TEST] Pose2 evaluations...")
-include("TestPoseAndPoint2Constraints.jl")
-println("[SUCCESS]")
-
-include("testBeehive2D_CliqByCliq.jl")
-
-# include("HexagonalLightGraphs.jl")
-
-@testset "[TEST] Pose2 evaluations..." begin
-  include("testDynPoint2D.jl")
+testfiles = [
+"testPoint2Point2WorldBearing.jl";
+"testBeehive2D_CliqByCliq.jl"; # special case debugging
+"testhigherdimroots.jl";
+"testManifoldsPose2Equivalent.jl";
+"testDidsonFunctions.jl";
+"testPoint2Point2.jl";
+"testPoint2Point2Init.jl";
+"testBasicPose2Stationary.jl";
+"TestPoseAndPoint2Constraints.jl";
+"testDynPoint2D.jl";
+"testBearingRange2D.jl";
+"testDeltaOdo.jl";
+"testFixedLagFG.jl";
+"testMultimodalRangeBearing.jl";
+"testDynPose2D.jl";
+"threeDimLinearProductTest.jl";
+"testPose3Pose3NH.jl";
+"testPartialXYH.jl";
+"testpartialpose3.jl";
+"testpackingconverters.jl";
+"TestDefaultFGInitialization.jl";
+"testAccumulateFactors.jl";
+"testDeadReckoningTether.jl"; ]
+@warn "must restore g2o parser test"
+# "testG2oParser.jl";  ]
+
+## Tests not ready yet
+# "HexagonalLightGraphs.jl"
+# "testCameraFunctions.jl"
+# "testmultiplefeatures.jl"
+
+
+for testf in testfiles
+  println("[TEST] $testf")
+  include(testf)
+  println("[SUCCESS]")
+  println()
+  println()
+  println()
 end
-
-include("testPoint2Point2WorldBearing.jl")
-
-include("testBearingRange2D.jl")
-
-include("testDeltaOdo.jl")
-
-include("testFixedLagFG.jl")
-
-include("testMultimodalRangeBearing.jl")
-
-include("testDynPose2D.jl")
-
-println("[TEST] Pose3 evaluations...")
-include("threeDimLinearProductTest.jl")
-println("[SUCCESS]")
-
-println("[TEST] ensure Pose3Pose3NH evaluations...")
-include("testPose3Pose3NH.jl")
-println("[SUCCESS]")
-
-@warn "Skipping JLD tests in RoME"
-# println("[TEST] saving to and loading from .jld2 file")
-# savejld(fg, file="tempfg.jld2" )
-# fgu = loadjld( file="tempfg.jld2" )
-# Base.rm("tempfg.jld2")
-# println("Success")
-
-println("[TEST] PartialPose3XYYaw evaluations...")
-include("testPartialXYH.jl")
-println("[SUCCESS]")
-
-println("[TEST] partial pose3 evaluations...")
-include("testpartialpose3.jl")
-println("[SUCCESS]")
-
-println("[TEST] packing converters...")
-include("testpackingconverters.jl")
-println("[SUCCESS]")
-
-include("TestDefaultFGInitialization.jl")
-
-include("testAccumulateFactors.jl")
-
-include("testDeadReckoningTether.jl")
-
-include("testG2oParser.jl")

test/testBasicPose2Stationary.jl

@@ -79,7 +79,8 @@ setValKDE!(fg, :x2, kde!(badval))
 # tree = wipeBuildNewTree!(fg, drawpdf=true, show=true)
 
 N = 100
-batchSolve!(fg, N=N)
+getSolverParams(fg).N = N
+solveTree!(fg)