Merge pull request #306 from JuliaRobotics/master

v0.7.2-rc1

.gitignore

@@ -0,0 +1,3 @@
+/dev
+Manifest.toml
+*.so

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.7.1"
+version = "0.7.2"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -37,10 +37,10 @@ Distributions = "0.20, 0.21, 0.22, 0.23"
 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.11, 0.12"
+IncrementalInference = "0.11, 0.12, 0.13"
 JLD2 = "0.1, 0.2, 0.3, 1.0"
 KernelDensityEstimate = "0.5.1, 0.6"
-Optim = "0.19, 0.20, 0.21"
+Optim = "0.19, 0.20, 0.21, 0.22"
 ProgressMeter = "1"
 Reexport = "0.2"
 Requires = "1.0"

compileRoME/Project.toml

@@ -0,0 +1,9 @@
+[deps]
+ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
+CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
+DistributedFactorGraphs = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
+IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480"
+KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
+RoME = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
+Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
+TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"

compileRoME/compileRoMESysimage.jl

@@ -0,0 +1,14 @@
+using Pkg
+
+Pkg.activate(@__DIR__)
+Pkg.instantiate()
+
+using PackageCompiler
+
+cd(@__DIR__)
+
+create_sysimage(:RoME, sysimage_path="RoMESysimage.so", precompile_execution_file="precompile_triggers.jl")
+
+
+## to use RoME with the newly created sysimage, start julia with:
+# julia -J RoMESysimage.so

compileRoME/precompile_triggers.jl

@@ -0,0 +1,2 @@
+import RoME
+include(joinpath(pkgdir(RoME), "test", "runtests.jl"))

src/CanonicalGraphs.jl

@@ -174,7 +174,7 @@ function generateCanonicalFG_Hexagonal(;fg::AbstractDFG=initfg(),
     @warn "autoinit is deprecated, use graphinit instead"
     autoinit
   end
-  return generateCanonicalFG_Circle(6, graphinit=graphinit, landmark=true, loopClosure=true)
+  return generateCanonicalFG_Circle(6, graphinit=graphinit, landmark=true, loopClosure=true; fg=fg)
 end
 
 

src/Deprecated.jl

@@ -1,4 +1,192 @@
 
+##==============================================================================
+## Delete at end v0.7.x
+
+import IncrementalInference: buildFactorDefault
+
+buildFactorDefault(::Type{Pose2Pose2}) = Pose2Pose2()
+buildFactorDefault(::Type{Pose2Point2}) = Pose2Point2()
+buildFactorDefault(::Type{Pose2Point2BearingRange}) = Pose2Point2BearingRange()
+buildFactorDefault(::Type{Point2Point2}) = Point2Point2()
+
+
+export Pose3Pose3NH, PackedPose3Pose3NH
+
+# -----------------------
+
+"""
+$(TYPEDEF)
+
+Obsolete, see issue https://github.com/JuliaRobotics/IncrementalInference.jl/issues/237.
+"""
+mutable struct Pose3Pose3NH <: IncrementalInference.FunctorPairwiseNH
+    Zij::Distribution
+    nullhypothesis::Distributions.Categorical
+    reuse::Vector{PP3REUSE}
+    Pose3Pose3NH() = new()
+    Pose3Pose3NH(s::Distribution, vh::Vector{Float64}) = new(s, Distributions.Categorical(vh), fill(PP3REUSE(), Threads.nthreads() )  )
+    # Pose3Pose3NH(s::SE3, c::Array{Float64,2}, vh::Float64) = new(s,c, Distributions.Categorical([(1.0-vh);vh]),SE3(0),SE3(0),SE3(0))
+    # Pose3Pose3NH(st::FloatInt, sr::Float64;vh::Float64=1.0) = new(SE3(0), [[st*Matrix{Float64}(LinearAlgebra.I, 3,3);zeros(3,3)];[zeros(3);sr*Matrix{Float64}(LinearAlgebra.I, 3,3)]], Distributions.Categorical([(1.0-vh);vh]),SE3(0),SE3(0),SE3(0))
+end
+function getSample(pp3::Pose3Pose3NH, N::Int=1)
+  return (rand(pp3.Zij, N), )
+end
+function (pp3::Pose3Pose3NH)(res::Array{Float64},
+            userdata,
+            idx::Int,
+            meas::Tuple,
+            wXi::Array{Float64,2},
+            wXj::Array{Float64,2}  )
+  #
+  reusethrid = pp3.reuse[Threads.threadid()]
+  fastpose3pose3residual!(reusethrid, res, idx, meas, wXi, wXj)
+  nothing
+end
+
+
+
+"""
+$(TYPEDEF)
+
+Obsolete, see issue https://github.com/JuliaRobotics/IncrementalInference.jl/issues/237.
+"""
+mutable struct PackedPose3Pose3NH <: IncrementalInference.PackedInferenceType
+  vecZij::Vector{Float64} # 3translations, 3rotation
+  vecCov::Vector{Float64}
+  dimc::Int
+  nullhypothesis::Vector{Float64}
+  PackedPose3Pose3NH() = new()
+  PackedPose3Pose3NH(x1::Vector{Float64},x2::Vector{Float64},x3::Int,x4::Vector{Float64}) = new(x1, x2, x3, x4)
+end
+
+function convert(::Type{Pose3Pose3NH}, d::PackedPose3Pose3NH)
+  qu = Quaternion(d.vecZij[4], d.vecZij[5:7])
+  se3val = SE3(d.vecZij[1:3], qu)
+  cov = reshapeVec2Mat(d.vecCov, d.dimc)
+  return Pose3Pose3NH( MvNormal(veeEuler(se3val), cov), d.nullhypothesis )
+end
+function convert(::Type{PackedPose3Pose3NH}, d::Pose3Pose3NH)
+  val = d.Zij.μ
+  se3val = SE3(val[1:3], Euler(val[4:6]...))
+  v1 = veeQuaternion(se3val)
+  v2 = d.Zij.Σ.mat
+  return PackedPose3Pose3NH(v1[:], v2[:], size(v2,1), d.nullhypothesis.p )
+end
+
+
+
+
+export Point2Point2WorldBearing, PackedPoint2Point2WorldBearing
+export PriorPoint2DensityNH, PackedPriorPoint2DensityNH
+
+
+"""
+$(TYPEDEF)
+
+TODO DEPRECATE
+"""
+mutable struct Point2Point2WorldBearing{T} <: IncrementalInference.FunctorPairwise where {T <: IIF.SamplableBelief}
+    Z::T
+    rangemodel::Rayleigh
+    # zDim::Tuple{Int, Int}
+    Point2Point2WorldBearing{T}() where T = new{T}()
+    Point2Point2WorldBearing{T}(x::T) where {T <: IIF.SamplableBelief} = new{T}(x, Rayleigh(100))
+end
+function Point2Point2WorldBearing(x::T) where {T <: IIF.SamplableBelief}
+  @warn "Point2Point2WorldBearing is being deprecated, use Point2, Point2Polar or Point2BearingRange instead."
+  Point2Point2WorldBearing{T}(x)
+end
+
+function getSample(pp2::Point2Point2WorldBearing, N::Int=1)
+  sp = Array{Float64,2}(undef, 2,N)
+  sp[1,:] = rand(pp2.Z,N)
+  sp[2,:] = rand(pp2.rangemodel,N)
+  return (sp, )
+end
+function (pp2r::Point2Point2WorldBearing)(
+          res::Array{Float64},
+          userdata::FactorMetadata,
+          idx::Int,
+          meas::Tuple,
+          pi::Array{Float64,2},
+          pj::Array{Float64,2} )
+  #
+  # noisy bearing measurement
+  z, r = meas[1][1,idx], meas[1][2,idx]
+  dx, dy = pj[1,idx]-pi[1,idx], pj[2,idx]-pi[2,idx]
+  res[1] = z - atan(dy,dx)
+  res[2] = r - norm([dx; dy])
+  nothing
+end
+
+
+
+
+"""
+$(TYPEDEF)
+
+Serialization type for `Point2Point2WorldBearing`.
+"""
+mutable struct PackedPoint2Point2WorldBearing  <: IncrementalInference.PackedInferenceType
+    str::String
+    # NOTE Not storing rangemodel which may cause inconsistencies if the implementation parameters change
+    PackedPoint2Point2WorldBearing() = new()
+    PackedPoint2Point2WorldBearing(x::String) = new(x)
+end
+function convert(::Type{PackedPoint2Point2WorldBearing}, d::Point2Point2WorldBearing)
+  return PackedPoint2Point2WorldBearing( string(d.Z) )
+end
+function convert(::Type{Point2Point2WorldBearing}, d::PackedPoint2Point2WorldBearing)
+  return Point2Point2WorldBearing( extractdistribution(d.str) )
+end
+
+"""
+$(TYPEDEF)
+
+Will be deprecated, use `addFactor!(.., nullhypo=)` instead (work in progress)
+"""
+mutable struct PriorPoint2DensityNH <: IncrementalInference.FunctorSingletonNH
+  belief::BallTreeDensity
+  nullhypothesis::Distributions.Categorical
+  PriorPoint2DensityNH() = new()
+  PriorPoint2DensityNH(belief, p::Distributions.Categorical) = new(belief, p)
+  PriorPoint2DensityNH(belief, p::Vector{Float64}) = new(belief, Distributions.Categorical(p))
+end
+function getSample(p2::PriorPoint2DensityNH, N::Int=1)
+  return (rand(p2.belief, N), )
+end
+
+"""
+$(TYPEDEF)
+
+Will be deprecated, use `addFactor!(.., nullhypo=)` instead (work in progress)
+"""
+mutable struct PackedPriorPoint2DensityNH <: IncrementalInference.PackedInferenceType
+    rpts::Vector{Float64} # 0rotations, 1translation in each column
+    rbw::Vector{Float64}
+    dims::Int
+    nh::Vector{Float64}
+    PackedPriorPoint2DensityNH() = new()
+    PackedPriorPoint2DensityNH(x1,x2,x3, x4) = new(x1, x2, x3, x4)
+end
+function convert(::Type{PriorPoint2DensityNH}, d::PackedPriorPoint2DensityNH)
+  return PriorPoint2DensityNH(
+            manikde!(reshapeVec2Mat(d.rpts, d.dims), d.rbw, (:Euclid, :Euclid)),
+            Distributions.Categorical(d.nh)  )
+end
+function convert(::Type{PackedPriorPoint2DensityNH}, d::PriorPoint2DensityNH)
+  return PackedPriorPoint2DensityNH( getPoints(d.belief)[:], getBW(d.belief)[:,1], Ndim(d.belief), d.nullhypothesis.p )
+end
+
+
+
+function PriorPoint2D(mu, cov, W)
+  @warn "PriorPoint2D(mu, cov, W) is deprecated, use PriorPoint{T}(T(...)) instead -- e.g. PriorPoint2{MvNormal}(MvNormal(...) or any other Distributions.Distribution type instead."
+  PriorPoint2{MvNormal{Float64}}(MvNormal(mu, cov))
+end
+
+
+##==============================================================================
 ## Delete at end v0.6.x
 
 export nextPose
@@ -482,11 +670,53 @@ end
 
 
 function basicFactorGraphExample(::Type{Pose2}=Pose2; addlandmark::Bool=true)
-  @warn "basicFactorGraphExample is deprecated, use loadCanonicalFG_TwoPoseOdo instead"
-  loadCanonicalFG_TwoPoseOdo(addlandmark=addlandmark)
+  @warn "basicFactorGraphExample is deprecated, use generateCanonicalFG_TwoPoseOdo instead"
+  generateCanonicalFG_TwoPoseOdo(addlandmark=addlandmark)
 end
 
 
+export projectParticles, ⊕
+
+
+
+
+# TODO -- stronger type safety required here
+# Project all particles (columns) Xval with Z, that is for all  SE3(Xval[:,i])*Z
+function projectParticles(Xval::Array{Float64,2}, Z::Distribution)
+  # TODO optimize for more speed with threads and better memory management
+  r,c = size(Xval)
+  RES = zeros(r,c) #*cz
+
+  ent, x = SE3(0), SE3(0)
+  ENT = rand( Z, c )
+  # ENT = rand( MvNormal(zeros(6), Cov), c )
+  j=1
+  # for j in 1:cz
+  for i in 1:c
+    x.R = TransformUtils.convert(SO3,Euler(Xval[4,i],Xval[5,i],Xval[6,i]))
+    x.t = Xval[1:3,i]
+    ent.R =  TransformUtils.convert(SO3, Euler(ENT[4:6,i]...)) # so3
+    ent.t = ENT[1:3,i]
+    # newval = Z*ent
+    # res = x*newval
+    res = x*ent
+    RES[1:r,i*j] = veeEuler(res)
+  end
+  # end
+  #
+  return RES
+end
+
+function ⊕(Xpts::Array{Float64,2}, z::Pose3Pose3)
+  @warn "⊕ use together with projectParticles is being replaced by approxConv.  The use of ⊕ will be revived in the future."
+  projectParticles(Xpts, z.Zij)
+end
+function ⊕(Xvert::Graphs.ExVertex, z::Pose3Pose3)
+  ⊕(getVal(Xvert), z)
+end
+
+
+
 # Project all particles (columns) Xval with Z, that is for all  SE3(Xval[:,i])*Z
 function projectParticles(Xval::Array{Float64,2}, Z::Array{Float64,2}, Cov::Array{Float64,2})
   # TODO optimize convert SE2 to a type

src/FactorGraphAnalysisTools.jl

@@ -1,4 +1,4 @@
-# Graph analysis tools
+# Graph analysis tools and support calculation tools
 
 
 function rangeErrMaxPoint2(fgl1::AbstractDFG, id1, fgl2::AbstractDFG ,id2)
@@ -30,3 +30,9 @@ function rangeCompAllPoses(
   end
   return ranges
 end
+
+
+
+
+
+#