Merge pull request #369 from JuliaRobotics/master

v0.11.2-rc1

Project.toml

@@ -1,8 +1,8 @@
 name = "RoME"
 uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
-keywords = ["SLAM", "state-estimation", "MM-iSAM", "inference", "robotics"]
+keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.11.1"
+version = "0.11.2"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -35,12 +35,12 @@ DistributedFactorGraphs = "0.10.2, 0.11"
 Distributions = "0.21, 0.22, 0.23, 0.24"
 DocStringExtensions = "0.7, 0.8"
 FileIO = "1.0.2, 1.1, 1.2"
-IncrementalInference = "0.17, 0.18"
+IncrementalInference = "0.17, 0.18, 0.19"
 JLD2 = "0.2, 0.3"
 KernelDensityEstimate = "0.5.1, 0.6"
 Optim = "0.22, 1.0"
 ProgressMeter = "1"
-Reexport = "0.2"
+Reexport = "0.2, 1.0"
 Requires = "1.0"
 Rotations = "0.12.1, 0.13, 1.0"
 TensorCast = "0.2, 0.3"

examples/KinematicJoints.jl

@@ -6,7 +6,7 @@ import IncrementalInference: getSample
 using TransformUtils
 
 
-mutable struct ZJoint <: AbstractRelativeFactor
+mutable struct ZJoint <: AbstractRelativeRoots
   Zij::Distribution
 end
 function getSample(el::ZJoint, N=1)
@@ -33,7 +33,7 @@ function (el::ZJoint)(res, userdata, idx, meas, xi, xj)
   nothing
 end
 
-mutable struct XJoint <: AbstractRelativeFactor
+mutable struct XJoint <: AbstractRelativeRoots
   Zij::Distribution
 end
 function getSample(el::XJoint, N=1)

examples/MITDatasetBatch.jl

@@ -39,7 +39,8 @@ function solve_batch(total_meas::Integer)
 
     # Solve the graph, and save a copy of the tree.
     saveDFG(fg, "$(getLogPath(fg))/fg-before-solve")
-    tree, smt, hist = solveTree!(fg, maxparallel=1000)
+    getSolverParams(fg).maxincidence = 1000
+    tree, smt, hist = solveTree!(fg)
     saveDFG(fg, "$(getLogPath(fg))/fg-after-solve")
     saveTree(tree, "$(getLogPath(fg))/tree$(mit_total_meas).jld2")
     drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt.pdf")

examples/MITDatasetFixedLag.jl

@@ -58,7 +58,8 @@ function go_fixedlag(initial_offset::Integer,
 
     # Solve the graph, and save a copy of the tree.
     saveDFG(fg, "$(getLogPath(fg))/fg-before-solve$(padded_step)")
-    tree, smt, hist = solveTree!(fg, maxparallel=1000)
+    getSolverParams(fg).maxincidence = 1000
+    tree, smt, hist = solveTree!(fg)
     saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
     saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
     drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")
@@ -99,7 +100,8 @@ function go_fixedlag(initial_offset::Integer,
         @info "Going for solve"
 
         # Solve the graph, and save a copy of the tree.
-        tree, smt, hist = solveTree!(fg, tree, maxparallel=1000)
+        getSolverParams(fg).maxincidence = 1000
+        tree, smt, hist = solveTree!(fg, tree)
         saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
         saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
         drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")

examples/MITDatasetIncremental.jl

@@ -89,7 +89,8 @@ function go(initial_offset::Integer, final_timestep::Integer, solve_stride::Inte
         @info "Going for solve"
 
         # Solve the graph, and save a copy of the tree.
-        tree, smt, hist = solveTree!(fg, tree, maxparallel=1000)
+        getSolverParams(fg).maxincidence = 1000
+        tree, smt, hist = solveTree!(fg, tree)
         saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
         saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
         drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")

examples/ManhattanDatasetBatch.jl

@@ -39,7 +39,8 @@ function solve_batch(total_meas::Integer)
 
     # Solve the graph, and save a copy of the tree.
     saveDFG(fg, "$(getLogPath(fg))/fg-before-solve")
-    tree, smt, hist = solveTree!(fg, maxparallel=1000)
+    getSolverParams(fg).maxincidence = 
+    tree, smt, hist = solveTree!(fg)
     saveDFG(fg, "$(getLogPath(fg))/fg-after-solve")
     saveTree(tree, "$(getLogPath(fg))/tree$(manhattan_total_meas).jld2")
     drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt.pdf")

examples/ManhattanDatasetFixedLag-FromBatch.jl

@@ -66,7 +66,8 @@ function go_fixedlag_frombatch(qfl_length_arg::Integer)
 
     # Solve the graph, and save a copy of the tree.
     saveDFG(fg, "$(getLogPath(fg))/fg-before-solve$(padded_step)")
-    tree, smt, hist = solveTree!(fg, maxparallel=1000)
+    getSolverParams(fg).maxincidence = 1000
+    tree, smt, hist = solveTree!(fg)
     saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
     saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
     drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")
@@ -94,7 +95,7 @@ function go_fixedlag_frombatch(qfl_length_arg::Integer)
         # Just store some quick plots, on another process
         remotecall((fgl, padded_stepl) -> begin
           @info "drawPoses, $(padded_stepl), for fg num variables=$(length(ls(fgl)))."
-          pl1 = drawPoses(fgl, spscale=0.6, lbls=false)
+          pl1 = plotSLAM2DPoses(fgl, dyadScale=0.6, lbls=false)
           pl1 |> PDF("$(getLogPath(fgl))/poses$(padded_stepl).pdf", 20cm, 10cm)
         end, rand(Categorical(nprocs()-1))+1, fg, padded_step)
 
@@ -107,7 +108,8 @@ function go_fixedlag_frombatch(qfl_length_arg::Integer)
         @info "Going for solve"
 
         # Solve the graph, and save a copy of the tree.
-        tree, smt, hist = solveTree!(fg, tree, maxparallel=1000)
+        getSolverParams(fg).maxincidence = 1000
+        tree, smt, hist = solveTree!(fg, tree)
         saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
         saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
         drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")

examples/ManhattanDatasetFixedLag-MargOnly.jl

@@ -55,7 +55,8 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt
 
     # Solve the graph, and save a copy of the tree.
     saveDFG(fg, "$(getLogPath(fg))/fg-before-solve$(padded_step)")
-    tree, smt, hist = solveTree!(fg, maxparallel=1000)
+    getSolverParams(fg).maxincidence = 1000
+    tree, smt, hist = solveTree!(fg)
     saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
     saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
     drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")
@@ -84,7 +85,7 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt
         # Just store some quick plots, on another process
         remotecall((fgl, padded_stepl) -> begin
           @info "drawPoses, $(padded_stepl), for fg num variables=$(length(ls(fgl)))."
-          pl1 = drawPoses(fgl, spscale=0.6, lbls=false)
+          pl1 = plotSLAM2DPoses(fgl, spscale=0.6, lbls=false)
           pl1 |> PDF("$(getLogPath(fgl))/poses$(padded_stepl).pdf", 20cm, 10cm)
         end, rand(Categorical(nprocs()-1))+1, fg, padded_step)
 
@@ -97,7 +98,8 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt
         @info "Going for solve"
 
         # Solve the graph, and save a copy of the tree.
-        tree, smt, hist = solveTree!(fg, maxparallel=1000)
+        getSolverParams(fg).maxincidence = 1000
+        tree, smt, hist = solveTree!(fg)
         saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
         saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
         drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")

examples/ManhattanDatasetFixedLag.jl

@@ -55,7 +55,8 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt
 
     # Solve the graph, and save a copy of the tree.
     saveDFG(fg, "$(getLogPath(fg))/fg-before-solve$(padded_step)")
-    tree, smt, hist = solveTree!(fg, maxparallel=1000)
+    getSolverParams(fg).maxincidence = 1000
+    tree, smt, hist = solveTree!(fg)
     saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
     saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
     drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")
@@ -96,7 +97,8 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt
         @info "Going for solve"
 
         # Solve the graph, and save a copy of the tree.
-        tree, smt, hist = solveTree!(fg, tree, maxparallel=1000)
+        getSolverParams(fg).maxincidence = 1000
+        tree, smt, hist = solveTree!(fg, tree)
         saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
         saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
         drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")

examples/ManhattanDatasetIncremental.jl

@@ -103,7 +103,8 @@ function go(initial_offset::Integer, final_timestep::Integer)
         @info "Going for solve"
 
         # Solve the graph, and save a copy of the tree.
-        tree, smt, hist = solveTree!(fg, tree, maxparallel=1000)
+        getSolverParams(fg).maxincidence = 1000
+        tree, smt, hist = solveTree!(fg, tree)
         saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
         saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
         drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")

examples/OutlierVsMultimodal.jl

@@ -286,14 +286,15 @@ end
 
 getSolverParams(fg).dbg = true
 saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)before"))
-tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg))
+getSolverParams(fg).maxincidence = 1000
+tree, smt, hist = solveTree!(fg, recordcliqs=ls(fg))
 saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_solve"))
 
 plfl1 = drawPosesLandms(fg, spscale=1.0,title=getLogPath(fg)*"\n$argstr", landmsPPE=:max, contour=true)
 plfl1 |> PDF(joinLogPath(fg, "plot_x$(POSEOFFSET+10)_solve.pdf"), 20cm, 17cm)
 
 if pargs["resolve"]
-  tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg))
+  tree, smt, hist = solveTree!(fg, recordcliqs=ls(fg))
   saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_resolve"))
   plfl1 = drawPosesLandms(fg, spscale=1.0,title=getLogPath(fg)*"\n$argstr", landmsPPE=:max, contour=true)
   plfl1 |> PDF(joinLogPath(fg, "plot_x$(POSEOFFSET+10)_resolve.pdf"), 20cm, 17cm)
@@ -306,7 +307,8 @@ end
 ## test solve
 
 getSolverParams(fg).dbg = true
-tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg)) #[:l9_0;:x14])
+getSolverParams(fg).maxincidence = 1000
+tree, smt, hist = solveTree!(fg, recordcliqs=ls(fg)) #[:l9_0;:x14])
 saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_final"))
 
 

src/SensorModels.jl

@@ -19,7 +19,7 @@ end
 """
 $(TYPEDEF)
 """
-mutable struct LinearRangeBearingElevation <: AbstractRelativeFactorMinimize
+mutable struct LinearRangeBearingElevation <: AbstractRelativeMinimize
   range::Normal
   bearing::Normal
   elev::Uniform

src/Slam.jl

@@ -258,7 +258,7 @@ function manageSolveTree!(dfg::AbstractDFG,
         dt_save1 = (time_ns()-t0)/1e9
         # constrain solve with the latest pose at the top
         # @show latestPose = intersect(getLastPoses(dfg, filterLabel=r"x\d", number=12), ls(dfg, r"x\d", solvable=1))[end]
-        tree, smt, hist = solveTree!(dfg, tree, maxparallel=1000) # , variableConstraints=[latestPose;]
+        tree, smt, hist = solveTree!(dfg, tree) # , variableConstraints=[latestPose;]
         dt_solve = (time_ns()-t0)/1e9
         !dbg ? nothing : saveDFG(dfg, joinpath(getLogPath(dfg), "fg_after_$(lasp)"))
 

src/factors/Bearing2D.jl

@@ -11,7 +11,7 @@ end
 
 Single dimension bearing constraint from Pose2 to Point2 variable.
 """
-struct Pose2Point2Bearing{B <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeFactorMinimize
+struct Pose2Point2Bearing{B <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize
     bearing::B
     reuse::Vector{P2P2BearingReuse}
     Pose2Point2Bearing{B}() where B = new{B}()
@@ -22,12 +22,12 @@ function getSample(pp2br::Pose2Point2Bearing, N::Int=1)
   return (reshape(rand(pp2br.bearing, N),1,N), )
 end
 # define the conditional probability constraint
-function (pp2br::Pose2Point2Bearing)(res::Array{Float64},
-                                     userdata::FactorMetadata,
-                                     idx::Int,
-                                     meas::Tuple,
-                                     xi::Array{Float64,2},
-                                     lm::Array{Float64,2}  )
+function (pp2br::Pose2Point2Bearing)( res::Array{Float64},
+                                      userdata::FactorMetadata,
+                                      idx::Int,
+                                      meas::Tuple,
+                                      xi::Array{Float64,2},
+                                      lm::Array{Float64,2}  )
   #
   reuse = pp2br.reuse[Threads.threadid()]
   reuse.measvec[1] = cos(meas[1][idx] + xi[3,idx])

src/factors/BearingRange2D.jl

@@ -7,7 +7,7 @@
 
 Bearing and Range constraint from a Pose2 to Point2 variable.
 """
-mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeFactorMinimize
+mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize
     bearing::B
     range::R
     Pose2Point2BearingRange{B,R}() where {B,R} = new{B,R}()

src/factors/DynPoint2D.jl

@@ -16,7 +16,7 @@ getSample(dp2v::DynPoint2VelocityPrior, N::Int=1) = (rand(dp2v.z,N), )
 """
 $(TYPEDEF)
 """
-mutable struct DynPoint2DynPoint2{T <: SamplableBelief} <: AbstractRelativeFactor
+mutable struct DynPoint2DynPoint2{T <: SamplableBelief} <: AbstractRelativeRoots
   z::T
   DynPoint2DynPoint2{T}() where {T <: SamplableBelief} = new{T}()
   DynPoint2DynPoint2(z1::T) where {T <: SamplableBelief} = new{T}(z1)
@@ -44,10 +44,10 @@ end
 """
 $(TYPEDEF)
 """
-mutable struct Point2Point2Velocity{T} <: IncrementalInference.AbstractRelativeFactorMinimize where {T <: Distribution}
+mutable struct Point2Point2Velocity{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize
   z::T
-  Point2Point2Velocity{T}() where {T <: Distribution} = new{T}()
-  Point2Point2Velocity(z1::T) where {T <: Distribution} = new{T}(z1)
+  Point2Point2Velocity{T}() where {T <: IIF.SamplableBelief} = new{T}()
+  Point2Point2Velocity(z1::T) where {T <: IIF.SamplableBelief} = new{T}(z1)
 end
 getSample(p2p2v::Point2Point2Velocity, N::Int=1) = (rand(p2p2v.z,N), )
 function (p2p2v::Point2Point2Velocity)(
@@ -77,7 +77,7 @@ end
 """
 $(TYPEDEF)
 """
-mutable struct PackedDynPoint2VelocityPrior <: IncrementalInference.PackedInferenceType
+mutable struct PackedDynPoint2VelocityPrior <: IIF.PackedInferenceType
   str::String
   PackedDynPoint2VelocityPrior() = new()
   PackedDynPoint2VelocityPrior(z1::String) = new(z1)

src/factors/DynPose2D.jl

@@ -22,7 +22,7 @@ getSample(dp2v::DynPose2VelocityPrior{T1,T2}, N::Int=1) where {T1 <: IIF.Samplab
 """
 $(TYPEDEF)
 """
-mutable struct DynPose2Pose2{T} <: IncrementalInference.AbstractRelativeFactor where {T <: IIF.SamplableBelief}
+mutable struct DynPose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeRoots
   Zpose::Pose2Pose2{T} #Zpose::T1
   # reuseres::Vector{Float64}
   partial::Tuple{Int,Int,Int}
@@ -32,13 +32,12 @@ end
 DynPose2Pose2(z1::T) where {T <: IIF.SamplableBelief} = DynPose2Pose2{T}(z1)
 
 getSample(vp2vp2::DynPose2Pose2, N::Int=1) = (rand(vp2vp2.Zpose.z,N), )
-function (vp2vp2::DynPose2Pose2{T})(
-                res::Array{Float64},
-                userdata::FactorMetadata,
-                idx::Int,
-                meas::Tuple,
-                wXi::Array{Float64,2},
-                wXj::Array{Float64,2}  ) where {T <: IIF.SamplableBelief}
+function (vp2vp2::DynPose2Pose2{T})(res::Array{Float64},
+                                    userdata::FactorMetadata,
+                                    idx::Int,
+                                    meas::Tuple,
+                                    wXi::Array{Float64,2},
+                                    wXj::Array{Float64,2}  ) where {T <: IIF.SamplableBelief}
   #
   vp2vp2.Zpose(res, userdata, idx, meas, wXi, wXj)
     # wXjhat = SE2(wxi[1:3,idx])*SE2(meas[1][1:3,idx])
@@ -115,7 +114,7 @@ end
 """
 $(TYPEDEF)
 """
-mutable struct DynPose2DynPose2{T <: IIF.SamplableBelief} <: AbstractRelativeFactor
+mutable struct DynPose2DynPose2{T <: IIF.SamplableBelief} <: AbstractRelativeRoots
   Z::T
   reuseres::Vector{Vector{Float64}}
   DynPose2DynPose2{T}() where {T <: IIF.SamplableBelief} = new{T}()

src/factors/InertialPose3.jl

@@ -146,7 +146,7 @@ $(TYPEDEF)
 
 Inertial Odometry version of preintegration procedure and used as a factor between InertialPose3 types for inertial navigation in factor graphs.
 """
-mutable struct InertialPose3 <: AbstractRelativeFactor #RoME.BetweenPoses
+mutable struct InertialPose3 <: AbstractRelativeRoots
   # Zij is entropy of veeLie15, pioc is preintegral measurements, pido is compensation gradients.
   Zij::Distribution
   pioc::InertialPose3Container

src/factors/MultipleFeaturesConstraint.jl

@@ -34,7 +34,7 @@ $(TYPEDEF)
 
 two sets of three feature sightings and a body to camera lever arm transform
 """
-mutable struct MultipleFeatures2D <: IncrementalInference.AbstractRelativeFactorMinimize
+mutable struct MultipleFeatures2D <: IIF.AbstractRelativeMinimize
   # treat these as angles positive X->Y, X forward, Y left and Z up
   xir1::Normal
   xir2::Normal

src/factors/MutablePose2Pose2.jl

@@ -8,10 +8,10 @@ export MutablePose2Pose2Gaussian, PackedMutablePose2Pose2Gaussian
 
 Specialized Pose2Pose2 factor type (Gaussian), which allows for rapid accumulation of odometry information as a branch on the factor graph.
 """
-mutable struct MutablePose2Pose2Gaussian  <: IIF.AbstractRelativeFactor
+mutable struct MutablePose2Pose2Gaussian  <: IIF.AbstractRelativeRoots
   Zij::MvNormal
   timestamp::DateTime
-  MutablePose2Pose2Gaussian(Zij::MvNormal=MvNormal(zeros(3),Matrix(Diagonal([0.01; 0.01; 0.001].^2))), timestamp::DateTime=now()) = new(Zij, timestamp)
+  MutablePose2Pose2Gaussian(Zij::MvNormal=MvNormal(zeros(3),diagm([0.01; 0.01; 0.001].^2)), timestamp::DateTime=now()) = new(Zij, timestamp)
 end
 function getSample(fct::MutablePose2Pose2Gaussian, N::Int=100)
   return (rand(fct.Zij, N), )