Merge pull request #1572 from JuliaRobotics/master

v0.30.1-rc1

Author: dehann

.github/workflows/ci.yml

@@ -6,6 +6,7 @@ on:
       - master
       - develop
       - release**
+  workflow_dispatch:
 jobs:
   test-stable:
     name: Julia ${{ matrix.version }} - ${{ matrix.os }} - ${{ matrix.arch }} - ${{ matrix.group }}

Project.toml

@@ -2,7 +2,7 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["MM-iSAMv2", "Bayes tree", "junction tree", "Bayes network", "variable elimination", "graphical models", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the Multimodal-iSAMv2 algorithm."
-version = "0.30.0"
+version = "0.30.1"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -29,6 +29,7 @@ MetaGraphs = "626554b9-1ddb-594c-aa3c-2596fe9399a5"
 NLSolversBase = "d41bc354-129a-5804-8e4c-c37616107c6c"
 NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56"
 Optim = "429524aa-4258-5aef-a3af-852621145aeb"
+OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d"
 ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca"
 Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd"
@@ -62,8 +63,9 @@ MetaGraphs = "0.7"
 NLSolversBase = "7.6"
 NLsolve = "3, 4"
 Optim = "1"
+OrderedCollections = "1"
 ProgressMeter = "1"
-RecursiveArrayTools = "2"
+RecursiveArrayTools = "2.31.1"
 Reexport = "1"
 Requires = "1"
 StaticArrays = "1"

src/Factors/GenericFunctions.jl

@@ -185,7 +185,7 @@ function convert(::Union{Type{<:AbstractFactor}, Type{<:ManifoldPrior}},
   M = DFG.getTypeFromSerializationModule(obj.varType) |> getManifold
 
   # TODO this is too excessive
-  e0 = identity_element(M)
+  e0 = getPointIdentity(M)
   # u0 = getPointIdentity(obj.varType)
   p = AMP.makePointFromCoords(M, obj.p, e0) #, u0)
 
@@ -204,7 +204,7 @@ end
 function samplePointPartial(M::AbstractDecoratorManifold,
                             z::Distribution,
                             partial::Vector{Int}, 
-                            p=identity_element(M), 
+                            p=getPointIdentity(M), 
                             retraction_method::AbstractRetractionMethod=ExponentialRetraction())
   dim = manifold_dimension(M)
   Xc = zeros(dim)

src/Factors/LinearRelative.jl

@@ -37,8 +37,6 @@ getDimension(::InstanceType{LinearRelative{N}}) where {N} = N
 # new and simplified interface for both nonparametric and parametric
 function (s::CalcFactor{<:LinearRelative})(z, x1, x2) 
   # TODO convert to distance(distance(x2,x1),z) # or use dispatch on `-` -- what to do about `.-`
-  # v0.21+, should return residual
-  !(z isa Vector{Float64}) && (@warn "H" z x1 x2)
   return z .- (x2 .- x1)
 end
 

src/IncrementalInference.jl

@@ -16,8 +16,10 @@ using Manifolds
 using RecursiveArrayTools: ArrayPartition
 export ArrayPartition
 
+using OrderedCollections: OrderedDict
+
 export ℝ, AbstractManifold
-export ProductRepr
+# export ProductRepr
 # common groups -- preferred defaults at this time.
 export TranslationGroup, RealCircleGroup
 # common non-groups -- TODO still teething problems to sort out in IIF v0.25-v0.26.

src/ManifoldSampling.jl

@@ -11,10 +11,11 @@ function sampleTangent end
 
 # Sampling MKD
 function sampleTangent(M::AbstractDecoratorManifold, x::ManifoldKernelDensity, p=mean(x))
-    # get legacy matrix of coordinates and selected labels
-    coords, lbls = sample(x.belief,1)
-    X = hat(x.manifold, p, coords)
-    return X
+  # get legacy matrix of coordinates and selected labels
+  #TODO make sure that when `sample` is replaced in MKD, coordinates is a vector
+  coords, lbls = sample(x.belief,1)
+  X = hat(x.manifold, p, coords[:])
+  return X
 end
 
 function sampleTangent(x::ManifoldKernelDensity, p=mean(x)) 
@@ -30,10 +31,6 @@ function sampleTangent(M::AbstractDecoratorManifold, z::Distribution, p=getPoint
   return hat(M, p, rand(z,1)[:]) #TODO find something better than (z,1)[:]
 end
 
-#TODO, re-evaluate this. special case for the RealCircleGroup to return a vector see https://github.com/JuliaManifolds/Manifolds.jl/issues/489 
-function sampleTangent(M::RealCircleGroup, z::Distribution, p=getPointIdentity(M)) 
-  return hat(M, [0.0], rand(z,1))
-end
 
 """
     $SIGNATURES

src/ManifoldsExtentions.jl

@@ -40,27 +40,16 @@ end
 
 #FIXME fix back to SA
 function getPointIdentity(G::SpecialOrthogonal{N},::Type{T}=Float64) where {N,T<:Real}
-#   return SMatrix{N,N, T}(I)
+  # return SMatrix{N,N, T}(I)
   return Matrix{T}(I, N, N)
 end
 
 function getPointIdentity(G::TranslationGroup{Tuple{N}},::Type{T}=Float64) where{N,T<:Real}
-#   return zeros(SVector{N,T})
+  # return zeros(SVector{N,T})
   return zeros(T,N)
 end
 
 function getPointIdentity(G::RealCircleGroup,::Type{T}=Float64) where T<:Real
     # return zero(T)
     return [zero(T)]
 end
-  
-
-
-# function Manifolds.allocate_result(G::SemidirectProductGroup, ::typeof(getPointIdentity))
-#   @warn "allocate_result(G::SemidirectProductGroup, ::typeof(getPointIdentity)"
-#   M = base_manifold(G)
-#   N, H = M.manifolds
-#   np = allocate_result(N, getPointIdentity)
-#   hp = allocate_result(H, getPointIdentity)
-#   return ArrayPartition(np, hp)
-# end

src/NumericalCalculations.jl

@@ -164,15 +164,6 @@ end
 # internal function to dispatch view on either vector or matrix, rows are dims and samples are columns
 _getindextuple(tup::Tuple, ind1::Int) = [getindex(t, ind1) for t in tup]
 
-# TODO, likely a shortlived function, and should be replaced with ccw.hypoParams::Tuple(hypo1, hypo2,...), made at construction and allows direct hypo lookup
-# DevNotes, also see new `hyporecipe` approach (towards consolidation CCW CPT FMd CF...)
-function _view(nt::NamedTuple, idxs::AbstractVector{<:Integer})
-  varParams = tuple([nt[i] for i in idxs]...)
-  tup = tuple(varParams...)
-  nms = keys(nt)[idxs]
-  return NamedTuple{nms,typeof(tup)}(tup)
-  # tuple([nt[i] for i in idxs]...)
-end
 
 function _buildCalcFactorMixture( ccwl::CommonConvWrapper,
                                   _fmd_,
@@ -225,8 +216,12 @@ function _buildCalcFactorLambdaSample(ccwl::CommonConvWrapper,
                                       _slack = nothing  )
   #
 
+  # TODO from obsolete _view:
+  # Should be replaced with ccw.hypoParams::Tuple(hypo1, hypo2,...), made at construction and allows direct hypo lookup
+  # DevNotes, also see new `hyporecipe` approach (towards consolidation CCW CPT FMd CF...)
+  
   # build a view to the decision variable memory
-  varValsHypo = _view(ccwl.params, cpt_.activehypo)
+  varValsHypo = ccwl.params[cpt_.activehypo]
   # tup = tuple(varParams...)
   # nms = keys(ccwl.params)[cpt_.activehypo]
   # varValsHypo = NamedTuple{nms,typeof(tup)}(tup)