Merge pull request #1791 from JuliaRobotics/master

fast forward

Author: dehann

NEWS.md

@@ -17,7 +17,8 @@ The list below highlights breaking changes according to normal semver workflow -
 - Deprecate `AbstractRelativeRoots`.
 - Standardization improvements surrounding weakdeps code extensions. 
 - Code quality improvements along wiht refactoring and reorganizing of file names and locations.
-- Restoring `DERelative` factors, although further fixes necessary beyond anticipated patch release v0.34.1.
+- Restoring `DERelative` factors, through v0.34.1 and v0.34.2.
+- Switching to weakdep AMD.jl for `ccolmod` dependency, part of Julia 1.10 upgrade.  Dropping `SuiteSparse_long` dependency.  Further fixes necessary to restore full user constrained tree variable order functionality.
 
 # Changes in v0.33
 

Project.toml

@@ -8,7 +8,6 @@ version = "0.34.1"
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 BSON = "fbb218c0-5317-5bc6-957e-2ee96dd4b1f0"
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
-BlockArrays = "8e7c35d0-a365-5155-bbbb-fb81a777f24e"
 Combinatorics = "861a8166-3701-5b0c-9a16-15d98fcdc6aa"
 DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8"
 Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"
@@ -67,7 +66,6 @@ IncrInfrInterpolationsExt = "Interpolations"
 [compat]
 ApproxManifoldProducts = "0.7, 0.8"
 BSON = "0.2, 0.3"
-BlockArrays = "0.16"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"
 DelimitedFiles = "1"

benchmark/runbenchmarks.jl

@@ -35,7 +35,7 @@ fg.solverParams.graphinit=false;
 addVariable!(fg, :x0, Pose2);
 addFactor!(fg, [:x0], PriorPose2(MvNormal([0.0,0,0], diagm([0.1,0.1,0.01].^2))));
 
-r = @timed IIF.solveGraphParametric!(fg; init=false);
+r = @timed IIF.solveGraphParametric!(fg; init=false, is_sparse=false);
 
 timed = [r];
 
@@ -44,7 +44,7 @@ for i = 1:14
     to = Symbol("x",i)
     addVariable!(fg, to, Pose2)
     addFactor!(fg, [fr,to], Pose2Pose2(MvNormal([10.0,0,pi/3], diagm([0.5,0.5,0.05].^2))))
-    r = @timed IIF.solveGraphParametric!(fg; init=false);
+    r = @timed IIF.solveGraphParametric!(fg; init=false, is_sparse=false);
     push!(timed, r)
 end
 
@@ -53,7 +53,7 @@ addVariable!(fg, :l1, RoME.Point2, tags=[:LANDMARK;]);
 addFactor!(fg, [:x0; :l1], Pose2Point2BearingRange(Normal(0.0,0.1), Normal(20.0, 1.0)));
 addFactor!(fg, [:x6; :l1], Pose2Point2BearingRange(Normal(0.0,0.1), Normal(20.0, 1.0)));
 
-r = @timed IIF.solveGraphParametric!(fg; init=false);
+r = @timed IIF.solveGraphParametric!(fg; init=false, is_sparse=false);
 push!(timed, r);
 
 getproperty.(timed, :time)

ext/IncrInfrDiffEqFactorExt.jl

@@ -8,8 +8,8 @@ import DifferentialEquations: solve
 using Dates
 
 using IncrementalInference
-import IncrementalInference: getSample, getManifold, DERelative
-import IncrementalInference: sampleFactor
+import IncrementalInference: DERelative, _solveFactorODE!
+import IncrementalInference: getSample, sampleFactor, getManifold
 
 using DocStringExtensions
 
@@ -174,12 +174,12 @@ function (cf::CalcFactor{<:DERelative})(measurement, X...)
     # need to recalculate new ODE (forward) for change in parameters (solving for 3rd or higher variable)
     solveforIdx = 2
     # use forward solve for all solvefor not in [1;2]
-    u0pts = getBelief(cf.fullvariables[1]) |> getPoints
+    # u0pts = getBelief(cf.fullvariables[1]) |> getPoints
     # update parameters for additional variables
     _solveFactorODE!(
       meas1,
       oderel.forwardProblem,
-      u0pts[cf._sampleIdx],
+      X[1], # u0pts[cf._sampleIdx],
       _maketuplebeyond2args(X...)...,
     )
   end
@@ -192,13 +192,52 @@ function (cf::CalcFactor{<:DERelative})(measurement, X...)
   #FIXME 
   res = zeros(size(X[2], 1))
   for i = 1:size(X[2], 1)
-    # diffop( test, reference )   <===>   ΔX = test \ reference
+    # diffop( reference?, test? )   <===>   ΔX = test \ reference
     res[i] = diffOp[i](X[solveforIdx][i], meas1[i])
   end
   return res
 end
 
 
+# # FIXME see #1025, `multihypo=` will not work properly yet
+# function getSample(cf::CalcFactor{<:DERelative})
+
+#   oder = cf.factor
+
+#   # how many trajectories to propagate?
+#   # @show getLabel(cf.fullvariables[2]), getDimension(cf.fullvariables[2])
+#   meas = zeros(getDimension(cf.fullvariables[2]))
+
+#   # pick forward or backward direction
+#   # set boundary condition
+#   u0pts = if cf.solvefor == 1
+#     # backward direction
+#     prob = oder.backwardProblem
+#     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
+#       convert(Tuple, getManifold(getVariableType(cf.fullvariables[1]))),
+#     )
+#     cf._legacyParams[2]
+#   else
+#     # forward backward
+#     prob = oder.forwardProblem
+#     # buffer manifold operations for use during factor evaluation
+#     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
+#       convert(Tuple, getManifold(getVariableType(cf.fullvariables[2]))),
+#     )
+#     cf._legacyParams[1]
+#   end
+
+#   i = cf._sampleIdx
+#   # solve likely elements
+#   # TODO, does this respect hyporecipe ???
+#   idxArr = (k -> cf._legacyParams[k][i]).(1:length(cf._legacyParams))
+#   _solveFactorODE!(meas, prob, u0pts[i], _maketuplebeyond2args(idxArr...)...)
+#   # _solveFactorODE!(meas, prob, u0pts, i, _maketuplebeyond2args(cf._legacyParams...)...)
+
+#   return meas, diffOp
+# end
+
+
 
 
 ## =========================================================================
@@ -221,15 +260,17 @@ function IncrementalInference.sampleFactor(cf::CalcFactor{<:DERelative}, N::Int
     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
       convert(Tuple, getManifold(getVariableType(cf.fullvariables[1]))),
     )
-    getBelief(cf.fullvariables[2]) |> getPoints
+    # getBelief(cf.fullvariables[2]) |> getPoints
+    cf._legacyParams[2]
   else
     # forward backward
     prob = oder.forwardProblem
     # buffer manifold operations for use during factor evaluation
     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
       convert(Tuple, getManifold(getVariableType(cf.fullvariables[2]))),
     )
-    getBelief(cf.fullvariables[1]) |> getPoints
+    # getBelief(cf.fullvariables[1]) |> getPoints
+    cf._legacyParams[1]
   end
 
   # solve likely elements

ext/WeakDepsPrototypes.jl

@@ -3,6 +3,9 @@
 function _ccolamd! end
 function _ccolamd end
 
+# DiffEq
+function _solveFactorODE! end
+
 # Flux.jl
 function MixtureFluxModels end
 

src/Factors/Circular.jl

@@ -23,7 +23,7 @@ DFG.getManifold(::CircularCircular) = RealCircleGroup()
 
 function (cf::CalcFactor{<:CircularCircular})(X, p, q)
   #
-  M = cf.manifold # getManifold(cf.factor)
+  M = getManifold(cf)
   return distanceTangent2Point(M, X, p, q)
 end
 
@@ -68,7 +68,7 @@ function getSample(cf::CalcFactor{<:PriorCircular})
 end
 
 function (cf::CalcFactor{<:PriorCircular})(m, p)
-  M = cf.manifold # getManifold(cf.factor)
+  M = getManifold(cf)
   Xc = vee(M, p, log(M, p, m))
   return Xc
 end

src/Factors/GenericFunctions.jl

@@ -37,7 +37,7 @@ end
 
 #::MeasurementOnTangent
 function distanceTangent2Point(M::SemidirectProductGroup, X, p, q)
-  q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) #for groups
+  q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X)) #for groups
   # return log(M, q, q̂)
   return vee(M, q, log(M, q, q̂))
   # return distance(M, q, q̂)
@@ -96,7 +96,7 @@ end
 
 # function (cf::CalcFactor{<:ManifoldFactor{<:AbstractDecoratorManifold}})(Xc, p, q)
 function (cf::CalcFactor{<:ManifoldFactor})(X, p, q)
-  return distanceTangent2Point(cf.manifold, X, p, q)
+  return distanceTangent2Point(cf.factor.M, X, p, q)
 end
 
 ## ======================================================================================
@@ -141,12 +141,20 @@ function getSample(cf::CalcFactor{<:ManifoldPrior})
   return point
 end
 
+function getFactorMeasurementParametric(fac::ManifoldPrior)
+  M = getManifold(fac)
+  dims = manifold_dimension(M)
+  meas = fac.p
+  iΣ = convert(SMatrix{dims, dims}, invcov(fac.Z))
+  meas, iΣ
+end
+
 #TODO investigate SVector if small dims, this is slower
 # dim = manifold_dimension(M)
 # Xc = [SVector{dim}(rand(Z)) for _ in 1:N]
 
 function (cf::CalcFactor{<:ManifoldPrior})(m, p)
-  M = cf.manifold # .factor.M
+  M = cf.factor.M
   # return log(M, p, m)
   return vee(M, p, log(M, p, m))
   # return distancePrior(M, m, p)
@@ -235,7 +243,7 @@ DFG.getManifold(f::ManifoldPriorPartial) = f.M
 
 function getSample(cf::CalcFactor{<:ManifoldPriorPartial})
   Z = cf.factor.Z
-  M = cf.manifold # getManifold(cf.factor)
+  M = getManifold(cf)
   partial = collect(cf.factor.partial)
 
   return (samplePointPartial(M, Z, partial),)

src/Factors/Mixture.jl

@@ -118,25 +118,36 @@ function sampleFactor(cf::CalcFactor{<:Mixture}, N::Int = 1)
   ## example case is old FluxModelsPose2Pose2 requiring velocity
   # FIXME better consolidation of when to pass down .mechanics, also see #1099 and #1094 and #1069
 
-  cf_ = CalcFactor(
+  cf_ = CalcFactorNormSq(
     cf.factor.mechanics,
     0,
     cf._legacyParams,
     cf._allowThreads,
     cf.cache,
     cf.fullvariables,
     cf.solvefor,
-    cf.manifold
+    cf.manifold,
+    cf.measurement,
+    nothing,
   )
   smpls = [getSample(cf_) for _ = 1:N]
   # smpls = Array{Float64,2}(undef,s.dims,N)
   #out memory should be right size first
   length(cf.factor.labels) != N ? resize!(cf.factor.labels, N) : nothing
   cf.factor.labels .= rand(cf.factor.diversity, N)
+  M = cf.manifold
+  
+  # mixture needs to be refactored so let's make it worse :-)
+  if cf.factor.mechanics isa AbstractPrior
+    samplef = samplePoint
+  elseif cf.factor.mechanics isa AbstractRelative
+    samplef = sampleTangent
+  end
+
   for i = 1:N
     mixComponent = cf.factor.components[cf.factor.labels[i]]
     # measurements relate to the factor's manifold (either tangent vector or manifold point)
-    setPointsMani!(smpls[i], rand(mixComponent, 1))
+    setPointsMani!(smpls,  samplef(M, mixComponent), i)
   end
 
   # TODO only does first element of meas::Tuple at this stage, see #1099

src/Factors/MsgPrior.jl

@@ -32,6 +32,7 @@ end
 getManifold(mp::MsgPrior{<:ManifoldKernelDensity}) = mp.Z.manifold
 getManifold(mp::MsgPrior) = mp.M
 
+#FIXME this will not work on manifolds
 (cfo::CalcFactor{<:MsgPrior})(z, x1) = z .- x1
 
 Base.@kwdef struct PackedMsgPrior <: AbstractPackedFactor

src/IncrementalInference.jl

@@ -221,7 +221,7 @@ include("parametric/services/ConsolidateParametricRelatives.jl")
 include("parametric/services/ParametricCSMFunctions.jl")
 include("parametric/services/ParametricUtils.jl")
 include("parametric/services/ParametricOptim.jl")
-include("parametric/services/ParametricManoptDev.jl")
+include("parametric/services/ParametricManopt.jl")
 include("services/MaxMixture.jl")
 
 #X-stroke