Merge pull request #1586 from JuliaRobotics/master

v0.30.3-rc1

Author: Affie

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.2"
+version = "0.30.3"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

src/Deprecated.jl

@@ -56,3 +56,110 @@ end
 @deprecate initManual!(w...;kw...) initVariable!(w...;kw...)
 
 
+##==============================================================================
+## Old parametric kept for comparason until code stabilize
+##==============================================================================
+
+
+
+"""
+    $SIGNATURES
+
+Batch solve a Gaussian factor graph using Optim.jl. Parameters can be passed directly to optim.
+Notes:
+  - Only :Euclid and :Circular manifolds are currently supported, own manifold are supported with `algorithmkwargs` (code may need updating though)
+"""
+function solveGraphParametric2(fg::AbstractDFG;
+                              computeCovariance::Bool = true,
+                              solvekey::Symbol=:parametric,
+                              autodiff = :forward,
+                              algorithm=Optim.BFGS,
+                              algorithmkwargs=(), # add manifold to overwrite computed one
+                              options = Optim.Options(allow_f_increases=true,
+                                                      time_limit = 100,
+                                                      # show_trace = true,
+                                                      # show_every = 1,
+                                                      ))
+
+  #Other options
+  # options = Optim.Options(time_limit = 100,
+  #                     iterations = 1000,
+  #                     show_trace = true,
+  #                     show_every = 1,
+  #                     allow_f_increases=true,
+  #                     g_tol = 1e-6,
+  #                     )
+  # Example for useing Optim's manifold functions
+  # mc_mani = IIF.MixedCircular(fg, varIds)
+  # alg = algorithm(;manifold=mc_mani, algorithmkwargs...)
+
+
+  varIds = listVariables(fg)
+
+  flatvar = FlatVariables(fg, varIds)
+
+  for vId in varIds
+    p = getVariableSolverData(fg, vId, solvekey).val[1]
+    flatvar[vId] = getCoordinates(getVariableType(fg,vId), p)
+  end
+
+  initValues = flatvar.X
+  # initValues .+= randn(length(initValues))*0.0001
+
+  alg = algorithm(; algorithmkwargs...)
+
+  cfd = calcFactorMahalanobisDict(fg)
+  tdtotalCost = Optim.TwiceDifferentiable((x)->_totalCost(fg, cfd, flatvar, x), initValues, autodiff = autodiff)
+
+  result = Optim.optimize(tdtotalCost, initValues, alg, options)
+  rv = Optim.minimizer(result)
+
+  Σ = if computeCovariance
+    H = Optim.hessian!(tdtotalCost, rv)
+    pinv(H)
+  else
+    N = length(initValues)
+    zeros(N,N)
+  end
+
+  d = Dict{Symbol,NamedTuple{(:val, :cov),Tuple{Vector{Float64},Matrix{Float64}}}}()
+
+  for key in varIds
+    r = flatvar.idx[key]
+    push!(d,key=>(val=rv[r],cov=Σ[r,r]))
+  end
+
+  return d, result, flatvar.idx, Σ
+end
+
+## ================================================================================================
+## Manifolds.jl Consolidation
+## TODO: Still to be completed and tested.
+## ================================================================================================
+# struct ManifoldsVector <: Optim.Manifold
+#   manis::Vector{Manifold}
+# end
+
+# Base.getindex(mv::ManifoldsVector, inds...) = getindex(mv.mani, inds...)
+# Base.setindex!(mv, X, inds...) =  setindex!(mv.mani, X, inds...)
+
+# function ManifoldsVector(fg::AbstractDFG, varIds::Vector{Symbol})
+#   manis = Bool[]
+#   for k = varIds
+#     push!(manis, getVariableType(fg, k) |> getManifold)
+#   end
+#   ManifoldsVector(manis)
+# end
+
+# function Optim.retract!(manis::ManifoldsVector, x)
+#   for (i,M) = enumerate(manis)
+#     x[i] = project(M, x[i])
+#   end
+#   return x 
+# end
+# function Optim.project_tangent!(manis::ManifoldsVector, G, x)
+#   for (i, M) = enumerate(manis)
+#     G[i] = project(M, x[i], G)
+#   end
+#   return G
+# end

src/ManifoldsExtentions.jl

@@ -1,3 +1,17 @@
+## ================================================================================================
+## AbstractPowerManifold with N as field to avoid excessive compiling time.
+## ================================================================================================
+struct NPowerManifold{𝔽, M<:AbstractManifold{𝔽}} <: AbstractPowerManifold{𝔽, M, NestedReplacingPowerRepresentation}
+  manifold::M
+  N::Int
+end
+
+Manifolds.get_iterator(M::NPowerManifold) = Base.OneTo(M.N)
+
+function Manifolds.manifold_dimension(M::NPowerManifold)
+  return manifold_dimension(M.manifold) * M.N
+end
+
 ## ================================================================================================
 ## ArrayPartition getPointIdentity (identity_element)
 ## ================================================================================================
@@ -16,18 +30,17 @@ function getPointIdentity(G::GroupManifold,::Type{T}=Float64) where T<:Real
   return error("getPointIdentity not implemented on G")
 end
 
-function getPointIdentity(G::ProductManifold,::Type{T}=Float64) where T<:Real
+function getPointIdentity(@nospecialize(G::ProductManifold),::Type{T}=Float64) where T<:Real
   return ArrayPartition(map(x->getPointIdentity(x,T), G.manifolds))
 end
 
-function getPointIdentity(M::Manifolds.PowerManifoldNestedReplacing,::Type{T}=Float64) where T<:Real
+function getPointIdentity(@nospecialize(M::PowerManifold),::Type{T}=Float64) where T<:Real
   N = Manifolds.get_iterator(M).stop
   return fill(getPointIdentity(M.manifold, T), N)
 end
 
-function getPointIdentity(M::PowerManifold,::Type{T}=Float64) where T<:Real
-  N = Manifolds.get_iterator(M).stop
-  return fill(getPointIdentity(M.manifold, T), N)
+function getPointIdentity(M::NPowerManifold,::Type{T}=Float64) where T<:Real
+  return fill(getPointIdentity(M.manifold, T), M.N)
 end
 
 function getPointIdentity(G::SemidirectProductGroup,::Type{T}=Float64) where T<:Real

src/ParametricCSMFunctions.jl

@@ -69,7 +69,7 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer)
   #fill in belief
   #TODO createBeliefMessageParametric(csmc.cliqSubFg, csmc.cliq, solvekey=opts.solvekey)
   cliqSeparatorVarIds = getCliqSeparatorVarIds(csmc.cliq)
-  #Fil in CliqueLikelihood
+  #Fill in CliqueLikelihood
   cliqlikelihood = calculateMarginalCliqueLikelihood(vardict, Σ, varIds, cliqSeparatorVarIds)
   # @info "$(csmc.cliq.id) clique likelihood message $(cliqlikelihood)"
   beliefMsg = LikelihoodMessage(sender=(; id=csmc.cliq.id.value,