Merge pull request #1713 from JuliaRobotics/23Q2/perf/bitstypes

Change points to StaticArrays

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.33.0"
+version = "0.34.0"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -48,7 +48,7 @@ TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 
 [compat]
-ApproxManifoldProducts = "0.6.3"
+ApproxManifoldProducts = "0.7"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"

src/ConsolidateParametricRelatives.jl

@@ -52,11 +52,12 @@ function solveFactorParameteric(
     # hasp ? getPPE(vari, key).suggested : calcMean(getBelief(vari, key))
     pt = calcMean(getBelief(vari, key))
 
-    return getCoordinates(getVariableType(vari), pt)
+    return collect(getCoordinates(getVariableType(vari), pt))
   end
 
   # overwrite specific src values from user
   coordVals = _getParametric.(getVariable.(dfg, varLbls), solveKey)
+
   for (srcsym, currval) in srcsym_vals
     coordVals[findfirst(varLbls .== srcsym)] = currval
   end

src/Deprecated.jl

@@ -1,3 +1,11 @@
+## ================================================================================================
+## ================================================================================================
+
+# TODO maybe upstream to DFG
+DFG.MeanMaxPPE(solveKey::Symbol, suggested::SVector, max::SVector, mean::SVector) =
+  DFG.MeanMaxPPE(solveKey, collect(suggested), collect(max), collect(mean))
+
+
 ## ================================================================================================
 ## Manifolds.jl Consolidation
 ## TODO: Still to be completed and tested.

src/Factors/Circular.jl

@@ -27,6 +27,11 @@ function (cf::CalcFactor{<:CircularCircular})(X, p, q)
   return distanceTangent2Point(M, X, p, q)
 end
 
+function getSample(cf::CalcFactor{<:CircularCircular})
+  # FIXME workaround for issue with manifolds CircularGroup, 
+  return [rand(cf.factor.Z)]
+end
+
 function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{CircularCircular})
   return Manifolds.RealCircleGroup()
 end
@@ -59,6 +64,7 @@ function getSample(cf::CalcFactor{<:PriorCircular})
   # JuliaManifolds/Manifolds.jl#415
   # no method similar(::Float64, ::Type{Float64})
   return samplePoint(cf.manifold, cf.factor.Z, [0.0])
+  # return [Manifolds.sym_rem(rand(cf.factor.Z))]
 end
 
 function (cf::CalcFactor{<:PriorCircular})(m, p)

src/ManifoldsExtentions.jl

@@ -132,21 +132,17 @@ function getPointIdentity(G::SemidirectProductGroup, ::Type{T} = Float64) where
   return ArrayPartition(np, hp)
 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 Matrix{T}(I, N, N)
+  return SMatrix{N, N, T}(I)
 end
 
 function getPointIdentity(
   G::TranslationGroup{Tuple{N}},
   ::Type{T} = Float64,
 ) where {N, T <: Real}
-  # return zeros(SVector{N,T})
-  return zeros(T, N)
+  return zeros(SVector{N,T})
 end
 
 function getPointIdentity(G::RealCircleGroup, ::Type{T} = Float64) where {T <: Real}
-  # return zero(T)
-  return [zero(T)]
+  return [zero(T)] #FIXME we cannot support scalars yet
 end

src/ParametricUtils.jl

@@ -52,7 +52,7 @@ end
 
 function Base.setindex!(
   flatVar::FlatVariables{T},
-  val::Vector{T},
+  val::AbstractVector{T},
   vId::Symbol,
 ) where {T <: Real}
   if length(val) == length(flatVar.idx[vId])
@@ -858,15 +858,15 @@ function initParametricFrom!(
     for v in getVariables(fg)
       fromvnd = getSolverData(v, fromkey)
       dims = getDimension(v)
-      getSolverData(v, parkey).val[1] .= fromvnd.val[1]
-      getSolverData(v, parkey).bw[1:dims, 1:dims] .= LinearAlgebra.I(dims)
+      getSolverData(v, parkey).val[1] = fromvnd.val[1]
+      getSolverData(v, parkey).bw[1:dims, 1:dims] = LinearAlgebra.I(dims)
     end
   else
     for var in getVariables(fg)
       dims = getDimension(var)
       μ, Σ = calcMeanCovar(var, fromkey)
-      getSolverData(var, parkey).val[1] .= μ
-      getSolverData(var, parkey).bw[1:dims, 1:dims] .= Σ
+      getSolverData(var, parkey).val[1] = μ
+      getSolverData(var, parkey).bw[1:dims, 1:dims] = Σ
     end
   end
 end
@@ -986,7 +986,7 @@ function autoinitParametric!(
 
     vnd.initialized = true
     #fill in ppe as mean
-    Xc = getCoordinates(getVariableType(xi), val)
+    Xc = collect(getCoordinates(getVariableType(xi), val))
     ppe = MeanMaxPPE(:parametric, Xc, Xc, Xc)
     getPPEDict(xi)[:parametric] = ppe
 

src/Variables/DefaultVariables.jl

@@ -50,5 +50,7 @@ $(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)
 
 #

src/services/EvalFactor.jl

@@ -370,16 +370,6 @@ function evalPotentialSpecific(
   return ccwl.varValsAll[ccwl.varidx[]], ipc
 end
 
-
-#TODO workaround for supporting bitstypes, need rewrite, can do with `PowerManifoldNestedReplacing` or similar
-function AMP.setPointsMani!(dest::AbstractArray{T}, src::AbstractArray{U}, destIdx, srcIdx=destIdx) where {T<:AbstractArray,U<:AbstractArray}
-  if isbitstype(T)
-    dest[destIdx] = src[srcIdx]
-  else
-    setPointsMani!(dest[destIdx],src[srcIdx])
-  end
-end
-
 # TODO `measurement` might not be properly wired up yet
 # TODO consider 1051 here to inflate proposals as general behaviour
 function evalPotentialSpecific(
@@ -477,10 +467,12 @@ function evalPotentialSpecific(
 
         setPointPartial!(
           mani,
-          addEntr[m],
+          addEntr,
           Msrc,
-          ccwl.measurement[m], # FIXME, measurements are tangents=>relative or points=>priors
+          ccwl.measurement, # FIXME, measurements are tangents=>relative or points=>priors
           partialCoords,
+          m,
+          m,
           asPartial,
         )
       else

src/services/FGOSUtils.jl

@@ -119,7 +119,7 @@ function manikde!(
   variableType::Union{InstanceType{<:InferenceVariable}, InstanceType{<:AbstractFactor}},
   pts::AbstractVector{P};
   kw...,
-) where {P <: Union{<:AbstractArray, <:Number, <:ProductRepr, <:Manifolds.ArrayPartition}}
+) where {P <: Union{<:AbstractArray, <:Number, <:Manifolds.ArrayPartition}}
   #
   M = getManifold(variableType)
   infoPerCoord = ones(AMP.getNumberCoords(M, pts[1]))

src/services/NumericalCalculations.jl

@@ -98,7 +98,11 @@ function _solveLambdaNumeric(
   r = if islen1
     Optim.optimize((x) -> (residual .= objResX(x); sum(residual .^ 2)), u0, Optim.BFGS())
   else
-    Optim.optimize((x) -> (residual .= objResX(x); sum(residual .^ 2)), u0)
+    Optim.optimize((x) -> (residual .= objResX(x); sum(residual .^ 2)), u0, Optim.Options(;iterations=1000))
+  end
+
+  if !Optim.converged(r)
+    @warn "Optim did not converge:" r maxlog=10
   end
 
   # 
@@ -113,6 +117,20 @@ function _solveLambdaNumeric(
   variableType::InferenceVariable,
   islen1::Bool = false,
 ) where {N_, F <: AbstractManifoldMinimize, S, T}
+
+  return _solveCCWNumeric_test_SA(fcttype, objResX, residual, u0, variableType, islen1)
+  # return _solveLambdaNumeric_test_optim_manifold(fcttype, objResX, residual, u0, variableType, islen1)
+
+end
+
+function _solveLambdaNumeric_original(
+  fcttype::Union{F, <:Mixture{N_, F, S, T}},
+  objResX::Function,
+  residual::AbstractVector{<:Real},
+  u0,#::AbstractVector{<:Real},
+  variableType::InferenceVariable,
+  islen1::Bool = false,
+) where {N_, F <: AbstractManifoldMinimize, S, T}
   #
   M = getManifold(variableType) #fcttype.M
   # the variable is a manifold point, we are working on the tangent plane in optim for now.
@@ -153,6 +171,93 @@ function _solveLambdaNumeric(
   return exp(M, ϵ, hat(M, ϵ, r.minimizer))
 end
 
+# 1.355700 seconds (11.78 M allocations: 557.677 MiB, 6.96% gc time)
+function _solveCCWNumeric_test_SA(
+  fcttype::Union{F, <:Mixture{N_, F, S, T}},
+  objResX::Function,
+  residual::AbstractVector{<:Real},
+  u0,#::AbstractVector{<:Real},
+  variableType::InferenceVariable,
+  islen1::Bool = false,
+) where {N_, F <: AbstractManifoldMinimize, S, T}
+  #
+  M = getManifold(variableType) #fcttype.M
+  # the variable is a manifold point, we are working on the tangent plane in optim for now.
+  # 
+  #TODO this is not general to all manifolds, should work for lie groups.
+  # ϵ = identity_element(M, u0)
+  ϵ = getPointIdentity(variableType)
+
+  X0c = zero(MVector{getDimension(M),Float64})
+  X0c .= vee(M, u0, log(M, ϵ, u0))
+
+  #TODO check performance
+  function cost(Xc)
+    X = hat(M, ϵ, Xc)
+    p = exp(M, ϵ, X)  
+    residual = objResX(p)
+    return sum(residual .^ 2)
+  end
+
+  alg = islen1 ? Optim.BFGS() : Optim.NelderMead()
+
+  r = Optim.optimize(cost, X0c, alg)
+  if !Optim.converged(r)
+    # TODO find good way for a solve to store diagnostics about number of failed converges etc.
+    @warn "Optim did not converge (maxlog=10):" r maxlog=10
+  end
+  return exp(M, ϵ, hat(M, ϵ, r.minimizer))
+end
+
+# sloooowwww and does not always converge, unusable slow with gradient
+# NelderMead 5.513693 seconds (38.60 M allocations: 1.613 GiB, 6.62% gc time)
+function _solveLambdaNumeric_test_optim_manifold(
+  fcttype::Union{F, <:Mixture{N_, F, S, T}},
+  objResX::Function,
+  residual::AbstractVector{<:Real},
+  u0,#::AbstractVector{<:Real},
+  variableType::InferenceVariable,
+  islen1::Bool = false,
+) where {N_, F <: AbstractManifoldMinimize, S, T}
+  #
+  M = getManifold(variableType) #fcttype.M
+  # the variable is a manifold point, we are working on the tangent plane in optim for now.
+  # 
+  #TODO this is not general to all manifolds, should work for lie groups.
+  ϵ = getPointIdentity(variableType)
+
+  function cost(p)
+    residual = objResX(p)
+    return sum(residual .^ 2)
+  end
+
+  alg = islen1 ? Optim.BFGS(;manifold=ManifoldWrapper(M)) : Optim.NelderMead(;manifold=ManifoldWrapper(M))
+  # alg = Optim.ConjugateGradient(; manifold=ManifoldWrapper(M))
+  # alg = Optim.BFGS(; manifold=ManifoldWrapper(M))
+
+  # r_backend = ManifoldDiff.TangentDiffBackend(
+  #   ManifoldDiff.FiniteDifferencesBackend()
+  # )
+  
+  # ## finitediff gradient
+  # function costgrad_FD!(X,p)
+  #   copyto!(X, ManifoldDiff.gradient(M, cost, p, r_backend))
+  #   X
+  # end
+
+  u0_m = allocate(M, u0)
+  u0_m .= u0 
+  # r = Optim.optimize(cost, costgrad_FD!, u0_m, alg)
+  r = Optim.optimize(cost, u0_m, alg)
+  
+  if !Optim.converged(r)
+    @warn "Optim did not converge:" r maxlog=10
+  end
+
+  return r.minimizer
+  # return exp(M, ϵ, hat(M, ϵ, r.minimizer))
+end
+
 #TODO Consolidate with _solveLambdaNumeric, see #1374
 function _solveLambdaNumericMeas(
   fcttype::Union{F, <:Mixture{N_, F, S, T}},
@@ -231,7 +336,7 @@ DevNotes
 function _buildCalcFactorLambdaSample(
   ccwl::CommonConvWrapper,
   smpid::Integer,
-  target = view(ccwl.varValsAll[ccwl.varidx[]][smpid], ccwl.partialDims),
+  target,
   measurement_ = ccwl.measurement;
   # fmd_::FactorMetadata = cpt_.factormetadata;
   _slack = nothing,
@@ -316,20 +421,39 @@ function _solveCCWNumeric!(
   islen1 = length(ccwl.partialDims) == 1 || ccwl.partial
   # islen1 = length(cpt_.X[:, smpid]) == 1 || ccwl.partial
 
+  if ccwl.partial
+    target = view(ccwl.varValsAll[ccwl.varidx[]][smpid], ccwl.partialDims)
+  else
+    target = ccwl.varValsAll[ccwl.varidx[]][smpid];
+  end
   # build the pre-objective function for this sample's hypothesis selection
-  unrollHypo!, target = _buildCalcFactorLambdaSample(ccwl, smpid; _slack = _slack)
+  unrollHypo!, _ = _buildCalcFactorLambdaSample(
+      ccwl, 
+      smpid, 
+      target;
+      _slack = _slack
+  )
 
   # broadcast updates original view memory location
   ## using CalcFactor legacy path inside (::CalcFactor)
-  _hypoObj = (x) -> (target .= x; unrollHypo!())
+
+  # _hypoObj = (x) -> (target[] = x; unrollHypo!())
+  function _hypoObj(x)
+    copyto!(target, x)
+    return unrollHypo!()
+  end
 
   # TODO small off-manifold perturbation is a numerical workaround only, make on-manifold requires RoME.jl #244
   # use all element dimensions : ==> 1:ccwl.xDim
-  target .+= _perturbIfNecessary(getFactorType(ccwl), length(target), perturb)
+  # target .+= _perturbIfNecessary(getFactorType(ccwl), length(target), perturb)
 
   sfidx = ccwl.varidx[]
   # do the parameter search over defined decision variables using Minimization
-  X = ccwl.varValsAll[sfidx][smpid][ccwl.partialDims]
+  if ccwl.partial
+    X = collect(view(ccwl.varValsAll[sfidx][smpid], ccwl.partialDims))
+  else
+    X = ccwl.varValsAll[sfidx][smpid][ccwl.partialDims]
+  end
   retval = _solveLambdaNumeric(
     getFactorType(ccwl), 
     _hypoObj, 
@@ -345,7 +469,11 @@ function _solveCCWNumeric!(
   end
 
   # insert result back at the correct variable element location
-  ccwl.varValsAll[sfidx][smpid][ccwl.partialDims] .= retval
+  if ccwl.partial
+    ccwl.varValsAll[sfidx][smpid][ccwl.partialDims] .= retval
+  else
+    copyto!(ccwl.varValsAll[sfidx][smpid], retval)
+  end
 
   return nothing
 end