wip w partials, sampleTangent

Author: dehann

src/Factors/GenericFunctions.jl

@@ -80,12 +80,14 @@ DFG.getManifold(f::ManifoldFactor) = f.M
 function getSample(cf::CalcFactor{<:ManifoldFactor{M, Z}}) where {M, Z}
   #TODO @assert dim == cf.factor.Z's dimension
   #TODO investigate use of SVector if small dims
-  if M isa ManifoldKernelDensity
-    ret = sample(cf.factor.Z.belief)[1]
-  else
-    ret = rand(cf.factor.Z)
-  end
-  # ret = sampleTangent(M, cf.factor.Z)
+  # if M isa ManifoldKernelDensity
+  #   ret = sample(cf.factor.Z.belief)[1]
+  # else
+  #   ret = rand(cf.factor.Z)
+  # end
+
+  # ASSUME this function is only used for RelativeFactors which must use measurements as tangents
+  ret = sampleTangent(cf.factor.M, cf.factor.Z)
   #return coordinates as we do not know the point here #TODO separate Lie group
   return ret
 end

src/ManifoldSampling.jl

@@ -23,7 +23,7 @@ function sampleTangent(x::ManifoldKernelDensity, p = mean(x))
 end
 
 # Sampling Distributions
-function sampleTangent(M::AbstractManifold, z::Distribution, p, basis::AbstractBasis)
+function sampleTangent(M::AbstractManifold, z::Distribution, p = getPointIdentity(M), basis::AbstractBasis = DefaultOrthogonalBasis())
   return get_vector(M, p, rand(z), basis)
 end
 

src/Variables/DefaultVariables.jl

@@ -16,7 +16,7 @@ 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.getPointIdentity(M_::Type{Position{N}}) where {N} = zeros(N) # identity_element(getManifold(M_), zeros(N)) 
+DFG.getPointIdentity(M_::Type{Position{N}}) where {N} = @SVector(zeros(N)) # identity_element(getManifold(M_), zeros(N)) 
 
 function Base.convert(
   ::Type{<:ManifoldsBase.AbstractManifold},

src/services/FGOSUtils.jl

@@ -122,6 +122,7 @@ function manikde!(
 ) where {P <: Union{<:AbstractArray, <:Number, <:Manifolds.ArrayPartition}}
   #
   M = getManifold(variableType)
+  # @info "pts" P typeof(pts[1]) pts[1]
   infoPerCoord = ones(AMP.getNumberCoords(M, pts[1]))
   return AMP.manikde!(M, pts; infoPerCoord, kw...)
 end

test/testSpecialEuclidean2Mani.jl

@@ -388,9 +388,23 @@ doautoinit!(fg, :x0)
 @test length(getPoints(getBelief(fg, :x0))) == getSolverParams(fg).N # 120
 # @info "PassThrough transfers the full point count to the graph, unless a product is calculated during the propagateBelief step."
 
+
+
+## check the partials magic
+
+dens, ipc = propagateBelief(fg,:x0,:)
+testv = deepcopy(getVariable(fg, :x0))
+setBelief!(testv, dens, true, ipc)
+
+
 ##
 
-solveGraph!(fg);
+smtasks = Task[]
+solveGraph!(fg; smtasks);
+# hists = fetchCliqHistoryAll!(smtasks)
+# printCSMHistoryLogical(hists)
+# hists_ = deepcopy(hists)
+# repeatCSMStep!(hists, 1, 6)
 
 @test 120 == length(getPoints(fg, :x0))