Merge pull request #1776 from JuliaRobotics/23Q3/fix/pospointtype

Fix getPointType of Position{N}

Author: dehann

src/Variables/DefaultVariables.jl

@@ -15,7 +15,7 @@ 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} = Vector{Float64}
+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)) 
 
 function Base.convert(

src/entities/ExtFactors.jl

@@ -19,7 +19,7 @@ DevNotes
 - FIXME Lots of consolidation and standardization to do, see RoME.jl #244 regarding Manifolds.jl.
 - TODO does not yet handle case where a factor spans across two timezones.
 """
-struct DERelative{T <: InferenceVariable, P, D} <: AbstractRelativeMinimize
+struct DERelative{T <: InferenceVariable, P, D} <: AbstractManifoldMinimize # AbstractRelativeMinimize
   domain::Type{T}
   forwardProblem::P
   backwardProblem::P

src/manifolds/services/ManifoldSampling.jl

@@ -24,7 +24,12 @@ end
 
 # Sampling Distributions
 # assumes M is a group and will break for Riemannian, but leaving that enhancement as TODO
-function sampleTangent(M::AbstractManifold, z::Distribution, p = getPointIdentity(M), basis::AbstractBasis = DefaultOrthogonalBasis())
+function sampleTangent(
+  M::AbstractManifold,
+  z::Distribution,
+  p = getPointIdentity(M),
+  basis::AbstractBasis = DefaultOrthogonalBasis()
+)
   return get_vector(M, p, rand(z), basis)
 end
 

src/manifolds/services/ManifoldsExtentions.jl

@@ -105,7 +105,7 @@ end
 
 # fallback 
 function getPointIdentity(G::GroupManifold, ::Type{T} = Float64) where {T <: Real}
-  return error("getPointIdentity not implemented on G")
+  return error("getPointIdentity not implemented on $G")
 end
 
 function getPointIdentity(

src/parametric/services/ParametricCSMFunctions.jl

@@ -38,7 +38,7 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer)
       vnd = getSolverData(getVariable(csmc.cliqSubFg, v), :parametric)
       # fill in the variable node data value
       logCSM(csmc, "$(csmc.cliq.id) up: updating $v : $val")
-      vnd.val[1] .= val.val
+      vnd.val[1] = val.val
       #calculate and fill in covariance
       #TODO rather broadcast than make new memory
       vnd.bw = val.cov
@@ -146,7 +146,7 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer)
         logCSM(csmc, "$(csmc.cliq.id) down: updating $v : $val"; loglevel = Logging.Info)
         vnd = getSolverData(getVariable(csmc.cliqSubFg, v), :parametric)
         #Update subfg variables
-        vnd.val[1] .= val.val
+        vnd.val[1] = val.val
         vnd.bw .= val.cov
       end
     else

src/services/GraphInit.jl

@@ -123,10 +123,11 @@ Notes:
 - Special carve out for multihypo cases, see issue 427.
 
 Development Notes:
-> Target factor is first (singletons) or second (dim 2 pairwise) variable vertex in `xi`.
-* TODO use DFG properly with local operations and DB update at end.
-* TODO get faster version of `isInitialized` for database version.
-* TODO: Persist this back if we want to here.
+- Target factor is first (singletons) or second (dim 2 pairwise) variable vertex in `xi`.
+- TODO use DFG properly with local operations and DB update at end.
+- TODO get faster version of `isInitialized` for database version.
+- TODO: Persist this back if we want to here.
+- TODO: init from just partials
 """
 function doautoinit!(
   dfg::AbstractDFG,

src/services/GraphProductOperations.jl

@@ -36,11 +36,12 @@ function propagateBelief(
   # make sure oldPoints vector has right length
   oldBel = getBelief(dfg, destlbl, solveKey)
   _pts = getPoints(oldBel, false)
-  oldPoints = if Npts(oldBel) <= N
-    _pts[1:N]
-  else
+  oldPoints = if Npts(oldBel) < N
     nn = N - length(_pts) # should be larger than 0
-    vcat(_pts, sample(oldBel, nn))
+    _pts_, = sample(oldBel, nn)
+    vcat(_pts, _pts_)
+  else
+    _pts[1:N]
   end
 
   # few more data requirements

src/services/NumericalCalculations.jl

@@ -362,10 +362,9 @@ function _solveCCWNumeric!(
   perturb::Real = 1e-10,
 ) where {N_, F <: AbstractRelative, S, T}
   #
-
+  
   #
   # thrid = Threads.threadid()
-
   smpid = ccwl.particleidx[]
   # cannot Nelder-Mead on 1dim, partial can be 1dim or more but being conservative.
   islen1 = length(ccwl.partialDims) == 1 || ccwl.partial
@@ -377,10 +376,11 @@ function _solveCCWNumeric!(
   #  a separate deepcopy of the destination (aka target) memory is necessary.
   #  Choosen solution is to splice together ccwl.varValsAll each time, with destination as 
   #  deepcopy but other input variables are just point to the source variable values directly. 
-  if ccwl.partial
-    target = view(ccwl.varValsAll[][ccwl.varidx[]][smpid], ccwl.partialDims)
+  target = if ccwl.partial  # FIXME likely type-instability on `typeof(target)`
+    # view(ccwl.varValsAll[][ccwl.varidx[]][smpid], ccwl.partialDims)
+    ccwl.varValsAll[][ccwl.varidx[]][smpid][ccwl.partialDims]
   else
-    target = ccwl.varValsAll[][ccwl.varidx[]][smpid];
+    ccwl.varValsAll[][ccwl.varidx[]][smpid]
   end
   # build the pre-objective function for this sample's hypothesis selection
   unrollHypo!, _ = _buildCalcFactorLambdaSample(
@@ -407,12 +407,13 @@ function _solveCCWNumeric!(
   # target .+= _perturbIfNecessary(getFactorType(ccwl), length(target), perturb)
   sfidx = ccwl.varidx[]
   # do the parameter search over defined decision variables using Minimization
-  if ccwl.partial
-    X = collect(view(ccwl.varValsAll[][sfidx][smpid], ccwl.partialDims))
-  else
-    X = ccwl.varValsAll[][sfidx][smpid][ccwl.partialDims]
-  end
-  # X = destVarVals[smpid]#[ccwl.partialDims]
+  X = ccwl.varValsAll[][sfidx][smpid][ccwl.partialDims]
+  # X = if ccwl.partial # TODO check for type-instability on `X`
+  #   collect(view(ccwl.varValsAll[][sfidx][smpid], ccwl.partialDims))
+  # else
+  #   ccwl.varValsAll[][sfidx][smpid][ccwl.partialDims]
+  # end
+  # # X = destVarVals[smpid]#[ccwl.partialDims]
 
   retval = _solveLambdaNumeric(
     getFactorType(ccwl), 
@@ -430,7 +431,13 @@ function _solveCCWNumeric!(
 
   # insert result back at the correct variable element location
   if ccwl.partial
-    ccwl.varValsAll[][sfidx][smpid][ccwl.partialDims] .= retval
+    # NOTE use workaround of TranslationGroup for coordinates on partial assignment
+    # FIXME consolidate to Manopt and upgrade to Riemannian (i.e. incl non-groups)
+    M = getManifold(ccwl) # TranslationGroup(length(ccwl.varValsAll[][sfidx][smpid]))
+    src = Vector{typeof(retval)}()
+    push!(src, retval)
+    setPointPartial!(M, ccwl.varValsAll[][sfidx], M, src, ccwl.partialDims, smpid, 1, true )
+    # ccwl.varValsAll[][sfidx][smpid][ccwl.partialDims] .= retval
   else
     # copyto!(ccwl.varValsAll[sfidx][smpid], retval)
     copyto!(ccwl.varValsAll[][sfidx][smpid][ccwl.partialDims], retval)

test/runtests.jl

@@ -5,30 +5,30 @@ TEST_GROUP = get(ENV, "IIF_TEST_GROUP", "all")
 # temporarily moved to start (for debugging)
 #...
 if TEST_GROUP in ["all", "tmp_debug_group"]
-include("testDERelative.jl")
 include("testSpecialOrthogonalMani.jl")
 include("testMultiHypo3Door.jl")
 include("priorusetest.jl")
 end
 
 if TEST_GROUP in ["all", "basic_functional_group"]
-  # more frequent stochasic failures from numerics
+# more frequent stochasic failures from numerics
 include("manifolds/manifolddiff.jl")
 include("manifolds/factordiff.jl")
 include("testSpecialEuclidean2Mani.jl")
 include("testEuclidDistance.jl")
 
-# regular testing
-include("testSphereMani.jl")
-include("testBasicManifolds.jl")
-
 # start as basic as possible and build from there
 include("typeReturnMemRef.jl")
 include("testDistributionsGeneric.jl")
-include("testHeatmapGridDensity.jl")
 include("testCliqSolveDbgUtils.jl")
 include("basicGraphsOperations.jl")
 
+# regular testing
+include("testSphereMani.jl")
+include("testBasicManifolds.jl")
+include("testDERelative.jl")
+include("testHeatmapGridDensity.jl")
+
 # include("TestModuleFunctions.jl")
 include("testCompareVariablesFactors.jl")
 include("saveconvertertypes.jl")

test/testBasicParametric.jl

@@ -124,7 +124,7 @@ end
 
 ##
 
-foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] .= x.second.val, pairs(d))
+foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second.val, pairs(d))
 
 
 # getSolverParams(fg).dbg=true
@@ -186,7 +186,7 @@ foreach(println, d)
 
 ##
 
-foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] .= x.second.val, pairs(d))
+foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second.val, pairs(d))
 
 # fg.solverParams.showtree = true
 # fg.solverParams.drawtree = true
@@ -228,7 +228,7 @@ for i in 0:10
   @test isapprox(d[sym].val[1], i, atol=1e-6)
 end
 
-foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] .= x.second.val, pairs(d))
+foreach(x->getSolverData(getVariable(fg,x.first),:parametric).val[1] = x.second.val, pairs(d))
 
 # fg.solverParams.showtree = true
 # fg.solverParams.drawtree = true