Start spllitting out IIFTypes

Author: Affie

IncrementalInferenceTypes/Project.toml

@@ -0,0 +1,26 @@
+name = "IncrementalInferenceTypes"
+uuid = "9808408f-4dbc-47e4-913c-6068b950e289"
+authors = ["Johannes Terblanche <Affie@users.noreply.github.com>"]
+version = "0.1.0"
+
+[deps]
+Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"
+Distributed = "8ba89e20-285c-5b6f-9357-94700520ee1b"
+DistributedFactorGraphs = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
+Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"
+DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
+Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
+RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
+StructTypes = "856f2bd8-1eba-4b0a-8007-ebc267875bd4"
+
+[compat]
+Dates = "1.11.0"
+Distributed = "1.11.0"
+DistributedFactorGraphs = "0.27.0"
+Distributions = "0.25.120"
+DocStringExtensions = "0.9.5"
+Manifolds = "=0.10.16"
+RecursiveArrayTools = "3.33.0"
+StaticArrays = "1.9.13"
+StructTypes = "1.11.0"

IncrementalInferenceTypes/src/IncrementalInferenceTypes.jl

@@ -0,0 +1,69 @@
+module IncrementalInferenceTypes
+
+using DistributedFactorGraphs
+using DocStringExtensions
+using Manifolds
+using Distributions
+using StaticArrays
+import StructTypes
+
+using Dates: now
+using Distributed: nprocs
+# using RecursiveArrayTools
+
+# export variable types
+export 
+    Position,
+    Position1,
+    Position2,
+    Position3,
+    Position4,
+    ContinuousScalar,
+    ContinuousEuclid,
+    Ciruclar
+
+#export factor types
+export 
+    Prior,
+    PackedPrior,
+    LinearRelative,
+    PackedLinearRelative,
+    CircularCircular,
+    PriorCircular,
+    PackedCircularCircular,
+    PackedPriorCircular
+
+# export packed distributions
+export
+    PackedCategorical,
+    PackedUniform,
+    PackedNormal,
+    PackedZeroMeanDiagNormal,
+    PackedZeroMeanFullNormal,
+    PackedDiagNormal,
+    PackedFullNormal,
+    PackedRayleigh
+
+export
+    SolverParams
+
+const IIFTypes = IncrementalInferenceTypes
+export IIFTypes
+
+# Variable Definitions
+include("variables/DefaultVariableTypes.jl")
+
+# Factor Definitions
+include("factors/DefaultPrior.jl")
+#FIXME maybe upgrade linear relative to this
+# include("factors/LinearRelative.jl")
+include("factors/Circular.jl")
+
+# Distribution Serialization
+include("serialization/entities/SerializingDistributions.jl")
+include("serialization/services/SerializingDistributions.jl")
+
+# solver params
+include("solverparams/SolverParams.jl")
+
+end # module IncrementalInferenceTypes

IncrementalInferenceTypes/src/factors/Circular.jl

@@ -0,0 +1,30 @@
+
+"""
+$(TYPEDEF)
+
+Factor between two Sphere1 variables.
+
+Related
+
+[`Sphere1`](@ref), [`PriorSphere1`](@ref), [`Polar`](@ref), [`ContinuousEuclid`](@ref)
+"""
+DFG.@defFactorType CircularCircular AbstractManifoldMinimize Manifolds.RealCircleGroup()
+
+
+"""
+$(TYPEDEF)
+
+Introduce direct observations on all dimensions of a Circular variable:
+
+Example:
+--------
+```julia
+PriorCircular( MvNormal([10; 10; pi/6.0], diagm([0.1;0.1;0.05].^2)) )
+```
+
+Related
+
+[`Circular`](@ref), [`Prior`](@ref), [`PartialPrior`](@ref)
+"""
+DFG.@defFactorType PriorCircular AbstractPrior Manifolds.RealCircleGroup()
+

IncrementalInferenceTypes/src/factors/DefaultPrior.jl

@@ -0,0 +1,29 @@
+"""
+$(TYPEDEF)
+
+Default prior on all dimensions of a variable node in the factor graph.  `Prior` is
+not recommended when non-Euclidean dimensions are used in variables.
+"""
+struct Prior{T} <: AbstractPrior
+  Z::T
+end
+DFG.getManifold(pr::Prior) = TranslationGroup(getDimension(pr.Z))
+
+"""
+$(TYPEDEF)
+
+Serialization type for Prior.
+"""
+Base.@kwdef mutable struct PackedPrior <: AbstractPackedFactor
+  Z::PackedSamplableBelief
+end
+
+function DFG.pack(d::Prior)
+  return PackedPrior(DFG.packDistribution(d.Z))
+end
+
+function DFG.unpack(d::PackedPrior)
+  return Prior(DFG.unpackDistribution(d.Z))
+end
+
+#

IncrementalInferenceTypes/src/factors/LinearRelative.jl

@@ -0,0 +1,33 @@
+
+"""
+$(TYPEDEF)
+
+Default linear offset between two scalar variables.
+
+```math
+X_2 = X_1 + η_Z
+```
+"""
+struct LinearRelative{T} <: AbstractManifoldMinimize
+  Z::T
+end
+
+DFG.getManifold(obs::LinearRelative) = TranslationGroup(getDimension(obs.Z))
+
+"""
+$(TYPEDEF)
+Serialization type for `LinearRelative` binary factor.
+"""
+Base.@kwdef mutable struct PackedLinearRelative <: AbstractPackedFactor
+  Z::PackedSamplableBelief
+end
+
+function DFG.pack(d::LinearRelative)
+  return PackedLinearRelative(DFG.packDistribution(d.Z))
+end
+
+function DFG.unpack(d::PackedLinearRelative)
+  return LinearRelative(DFG.unpackDistribution(d.Z))
+end
+
+#

IncrementalInferenceTypes/src/serialization/entities/SerializingDistributions.jl

@@ -0,0 +1,47 @@
+
+Base.@kwdef struct PackedCategorical <: PackedSamplableBelief
+  _type::String = "IncrementalInferenceTypes.PackedCategorical"
+  p::Vector{Float64} = [1.0;]
+end
+
+Base.@kwdef mutable struct PackedUniform <: PackedSamplableBelief
+  _type::String = "IncrementalInferenceTypes.PackedUniform"
+  a::Float64 = 0.0
+  b::Float64 = 1.0
+  PackedSamplableTypeJSON::String = "IncrementalInferenceTypes.PackedUniform"
+end
+
+Base.@kwdef struct PackedNormal <: PackedSamplableBelief
+  _type::String = "IncrementalInferenceTypes.PackedNormal"
+  mu::Float64 = 0.0
+  sigma::Float64 = 1.0
+end
+
+Base.@kwdef struct PackedZeroMeanDiagNormal <: PackedSamplableBelief
+  _type::String = "IncrementalInferenceTypes.PackedZeroMeanDiagNormal"
+  diag::Vector{Float64} = ones(1)
+end
+
+Base.@kwdef struct PackedZeroMeanFullNormal <: PackedSamplableBelief
+  _type::String = "IncrementalInferenceTypes.PackedZeroMeanFullNormal"
+  cov::Vector{Float64} = ones(1)
+end
+
+Base.@kwdef mutable struct PackedDiagNormal <: PackedSamplableBelief
+  _type::String = "IncrementalInferenceTypes.PackedDiagNormal"
+  mu::Vector{Float64} = zeros(1)
+  diag::Vector{Float64} = ones(1)
+end
+
+Base.@kwdef struct PackedFullNormal <: PackedSamplableBelief
+  _type::String = "IncrementalInferenceTypes.PackedFullNormal"
+  mu::Vector{Float64} = zeros(1)
+  cov::Vector{Float64} = ones(1)
+end
+
+Base.@kwdef struct PackedRayleigh <: PackedSamplableBelief
+  _type::String = "IncrementalInferenceTypes.PackedRayleigh"
+  sigma::Float64 = 1.0
+end
+
+#

IncrementalInferenceTypes/src/serialization/services/SerializingDistributions.jl

@@ -0,0 +1,30 @@
+
+## Distributions to JSON/Packed types
+
+DFG.packDistribution(dtr::Categorical) = PackedCategorical(; p = dtr.p)
+DFG.packDistribution(dtr::Uniform) = PackedUniform(; a = dtr.a, b = dtr.b)
+DFG.packDistribution(dtr::Normal) = PackedNormal(; mu = dtr.μ, sigma = dtr.σ)
+DFG.packDistribution(dtr::ZeroMeanDiagNormal) = PackedZeroMeanDiagNormal(; diag = dtr.Σ.diag)
+DFG.packDistribution(dtr::ZeroMeanFullNormal) = PackedZeroMeanFullNormal(; cov = dtr.Σ.mat[:])
+DFG.packDistribution(dtr::DiagNormal) = PackedDiagNormal(; mu = dtr.μ, diag = dtr.Σ.diag)
+DFG.packDistribution(dtr::FullNormal) = PackedFullNormal(; mu = dtr.μ, cov = dtr.Σ.mat[:])
+DFG.packDistribution(dtr::Rayleigh) = PackedRayleigh(; sigma = dtr.σ)
+
+## Unpack JSON/Packed to Distribution types
+
+DFG.unpackDistribution(dtr::PackedCategorical) = Categorical(dtr.p ./ sum(dtr.p))
+DFG.unpackDistribution(dtr::PackedUniform) = Uniform(dtr.a, dtr.b)
+DFG.unpackDistribution(dtr::PackedNormal) = Normal(dtr.mu, dtr.sigma)
+function DFG.unpackDistribution(dtr::PackedZeroMeanDiagNormal)
+  return MvNormal(LinearAlgebra.Diagonal(map(abs2, sqrt.(dtr.diag))))
+end # sqrt.(dtr.diag)
+function DFG.unpackDistribution(dtr::PackedZeroMeanFullNormal)
+  d = round(Int, sqrt(size(dtr.cov)[1]))
+  return MvNormal(reshape(dtr.cov, d, d))
+end
+DFG.unpackDistribution(dtr::PackedDiagNormal) = MvNormal(dtr.mu, sqrt.(dtr.diag))
+function DFG.unpackDistribution(dtr::PackedFullNormal)
+  return MvNormal(dtr.mu, reshape(dtr.cov, length(dtr.mu), :))
+end
+DFG.unpackDistribution(dtr::PackedRayleigh) = Rayleigh(dtr.sigma)
+

src/entities/SolverParams.jl renamed to IncrementalInferenceTypes/src/solverparams/SolverParams.jl

@@ -11,7 +11,7 @@ Dev Notes
 """
 Base.@kwdef mutable struct SolverParams <: DFG.AbstractParams
   dimID::Int = 0
-  reference::NothingUnion{Dict{Symbol, Tuple{Symbol, Vector{Float64}}}} = nothing
+  reference::Union{Nothing, Dict{Symbol, Tuple{Symbol, Vector{Float64}}}} = nothing
   stateless::Bool = false
   """ Quasi fixed length """
   qfl::Int = (2^(Sys.WORD_SIZE - 1) - 1)
@@ -45,7 +45,7 @@ Base.@kwdef mutable struct SolverParams <: DFG.AbstractParams
   """ should Distributed.jl tree solve compute features be used """
   multiproc::Bool = 1 < nprocs()    
   """ "/tmp/caesar/logs/$(now())" # unique temporary file storage location for a solve """
-  logpath::String = joinpath(tempdir(),"caesar","logs","$(now(UTC))") 
+  logpath::String = joinpath(tempdir(),"caesar","logs","$(now(DFG.UTC))") 
   """ default to graph-based initialization of variables """
   graphinit::Bool = true            
   """ init variables on the tree """
@@ -76,6 +76,9 @@ end
 
 StructTypes.omitempties(::Type{SolverParams}) = (:reference,)
 
-
-convert(::Type{SolverParams}, ::NoSolverParams) = SolverParams()
 #
+Base.convert(::Type{SolverParams}, ::NoSolverParams) = begin 
+  @warn "FIXME Why converting NoSolverParams to SolverParams?"
+  SolverParams()
+end
+

IncrementalInferenceTypes/src/variables/DefaultVariableTypes.jl

@@ -0,0 +1,58 @@
+#FIXME This is discouraged in the julia style guide, rather standardize to instance or type
+const InstanceType{T} = Union{Type{<:T}, <:T}
+
+## Euclid 1
+
+"""
+    $TYPEDEF
+
+Continuous Euclidean variable of dimension `N` representing a Position in cartesian space.
+"""
+struct Position{N} <: InferenceVariable end
+
+Position(N::Int) = Position{N}()
+
+# not sure if these overloads are necessary since DFG 775?
+DFG.getManifold(::InstanceType{Position{N}}) where {N} = TranslationGroup(N)
+function DFG.getDimension(val::InstanceType{Position{N}}) where {N}
+  return manifold_dimension(getManifold(val))
+end
+DFG.getPointType(::Type{Position{N}}) where {N} = SVector{N, Float64}
+DFG.getPointIdentity(M_::Type{Position{N}}) where {N} = @SVector(zeros(N)) # identity_element(getManifold(M_), zeros(N)) 
+
+
+#
+
+"""
+$(TYPEDEF)
+
+Most basic continuous scalar variable in a `::DFG.AbstractDFG` object.
+
+Alias of `Position{1}`
+"""
+const ContinuousScalar = Position{1}
+const ContinuousEuclid{N} = Position{N}
+
+const Position1 = Position{1}
+const Position2 = Position{2}
+const Position3 = Position{3}
+const Position4 = Position{4}
+
+#TODO maybe just use @defVariable for all Position types?
+# @defVariable Position1 TranslationGroup(1) @SVector(zeros(1))
+# @defVariable Position2 TranslationGroup(2) @SVector(zeros(2))
+# @defVariable Position3 TranslationGroup(3) @SVector(zeros(3))
+# @defVariable Position4 TranslationGroup(4) @SVector(zeros(4))
+
+## Circular
+
+"""
+$(TYPEDEF)
+
+Circular is a `Manifolds.Circle{ℝ}` mechanization of one rotation, with `theta in [-pi,pi)`.
+"""
+@defVariable Circular RealCircleGroup() [0.0;]
+#TODO This is an example of what we want working, possible issue upstream in Manifolds.jl
+# @defVariable Circular RealCircleGroup() Scalar(0.0)
+
+#