Merge pull request #1705 from JuliaRobotics/23Q3/perf/bitstypes

Towards StaticArrays Support

Author: Affie

src/ManifoldSampling.jl

@@ -66,7 +66,8 @@ end
 function samplePoint(
   M::AbstractDecoratorManifold,
   sbelief::ManifoldKernelDensity,
-  p = identity_element(M, mean(sbelief)),
+  # p = identity_element(M, mean(sbelief)), # 8.671254 seconds (82.64 M allocations: 3.668 GiB, 7.50% gc time)
+  p = getPointIdentity(M), #6.713209 seconds (66.42 M allocations: 3.141 GiB, 7.52% gc time)
   retraction_method::AbstractRetractionMethod = ExponentialRetraction(),
 )
   X = sampleTangent(M, sbelief, p)

src/services/EvalFactor.jl

@@ -102,7 +102,8 @@ function addEntropyOnManifold!(
   # preallocate 
   T = number_eltype(points[1])
   Xc = zeros(T, manifold_dimension(M))
-  X = get_vector(M, points[1], Xc, DefaultOrthogonalBasis())
+  #allocate to change SMatrix to MMatrix
+  X = allocate(get_vector(M, points[1], Xc, DefaultOrthogonalBasis()))
 
   for idx = 1:length(points)
     # build tangent coordinate random
@@ -115,8 +116,8 @@ function addEntropyOnManifold!(
     get_vector!(M, X, points[idx], Xc, DefaultOrthogonalBasis())
     #update point
     # exp!(M, points[idx], points[idx], X)
-    retract!(M, points[idx], points[idx], X)
-    # points[idx] = exp(M, points[idx], X)
+    # retract!(M, points[idx], points[idx], X)
+    points[idx] = retract(M, points[idx], X)
 
   end
   #
@@ -369,6 +370,16 @@ 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(
@@ -436,7 +447,7 @@ function evalPotentialSpecific(
     for m in (1:length(addEntr))[ahmask]
       # FIXME, selection for all measurement::Tuple elements
       # @info "check broadcast" ccwl.usrfnc! addEntr[m] ccwl.measurement[1][m]
-      setPointsMani!(addEntr[m], ccwl.measurement[m])
+      setPointsMani!(addEntr, ccwl.measurement, m)
       # addEntr[m] = ccwl.measurement[m][1]
     end
     # ongoing part of RoME.jl #244

src/services/NumericalCalculations.jl

@@ -385,7 +385,7 @@ function _solveCCWNumeric!(
   ## using CalcFactor legacy path inside (::CalcFactor)
   # _hypoObj = (x) -> (target.=x; unrollHypo!())
   function _hypoObj(x)
-    target[] .= x
+    target[] = x
     return unrollHypo!()
   end
 
@@ -413,7 +413,7 @@ function _solveCCWNumeric!(
   # end
 
   # FIXME insert result back at the correct variable element location
-  ccwl.varValsAll[sfidx][smpid] .= retval
+  ccwl.varValsAll[sfidx][smpid] = retval
 
   return nothing
 end