Merge pull request #1345 from JuliaRobotics/21Q3/maint/upstgenerators

Standardize to TranslationGroup

Author: dehann

src/FGOSUtils.jl

@@ -235,6 +235,12 @@ function fifoFreeze!(dfg::AbstractDFG)
 end
 
 
+DFG.getPoint(typ::InferenceVariable, w...;kw...) = getPoint(typeof(typ), w...;kw...)
+DFG.getCoordinates(typ::InferenceVariable, w...;kw...) = getCoordinates(typeof(typ), w...;kw...)
+
+# WIP
+# _getMeasurementRepresentation(::AbstractPrior, coord::AbstractVector{<:Number}) = 
+
 
 """
     $SIGNATURES
@@ -267,7 +273,7 @@ function calcPPE( var::DFGVariable,
   # calculate point
 
   ## TODO make PPE only use getCoordinates for now (IIF v0.25)
-  Pme_ = getCoordinates(typeof(varType),Pme)
+  Pme_ = getCoordinates(varType,Pme)
   # Pma_ = getCoordinates(M,Pme)
 
   # suggested, max, mean, current time

src/FactorGraph.jl

@@ -629,7 +629,7 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
       gradients = FactorGradientsCached!(usrfnc, tuple(varTypes[hypoidxs]...), measurement_, tuple(pts_[hypoidxs]...), _blockRecursion=true);
     end
   catch e
-    @warn "Unable to create measurements and gradients for $usrfnc during prep of CCW, falling back on no-partial information assumption.  Enable @debug printing to see the error."
+    @warn "Unable to create measurements and gradients for $usrfnc during prep of CCW, falling back on no-partial information assumption.  Enable ENV[\"JULIA_DEBUG\"] = \"IncrementalInference\" for @debug printing to see the error."
     @debug(e)
   end
 

src/Factors/EuclidDistance.jl

@@ -14,7 +14,8 @@ end
 
 EuclidDistance(::UniformScaling=LinearAlgebra.I) = EuclidDistance(Normal())
 
-getManifold(::InstanceType{EuclidDistance}) = Euclidean(1)
+# consider a different group?
+getManifold(::InstanceType{EuclidDistance}) = TranslationGroup(1)
 getDimension(::InstanceType{<:EuclidDistance}) = 1
 
 
@@ -30,7 +31,7 @@ function (s::CalcFactor{<:EuclidDistance})(z, x1, x2)
 end
 
 
-Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{EuclidDistance}) = Manifolds.Euclidean(1)
+Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{EuclidDistance}) = Manifolds.TranslationGroup(1)
 
 
 """

src/Factors/LinearRelative.jl

@@ -50,7 +50,7 @@ end
 
 
 
-Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{LinearRelative{N}}) where N = Manifolds.Euclidean(N) 
+Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{LinearRelative{N}}) where N = Manifolds.TranslationGroup(N) 
 
 
 

src/IncrementalInference.jl

@@ -69,6 +69,8 @@ import DistributedFactorGraphs: rebuildFactorMetadata!
 import DistributedFactorGraphs: getDimension, getManifold, getPointType, getPointIdentity
 import DistributedFactorGraphs: getPPE, getPPEDict
 import DistributedFactorGraphs: getFactorOperationalMemoryType
+import DistributedFactorGraphs: getPoint, getCoordinates
+
 
 # will be deprecated in IIF
 import DistributedFactorGraphs: isSolvable

src/ParametricUtils.jl

@@ -39,34 +39,37 @@ end
 """
     $SIGNATURES
 
-Returns the parametric measurement for a factor as a tuple (measurement, inverse covariance) for parametric inference (assumign Gaussian).
-Defaults to find the parametric measurement at field `Z` followed by `z`.
+Returns the parametric measurement for a factor as a tuple (measurement, inverse covariance) for parametric inference (assuming Gaussian).
+Defaults to find the parametric measurement at field `Zij`, `Z`, or `z`.
 
 Notes
 - Users should overload this method should their factor not default to `.Z<:ParametricType` or `.z<:ParametricType`
+- First design choice was to restrict this function to returning coordinates
+  - See https://github.com/JuliaRobotics/RoME.jl/issues/465
+  - Pay attention to which tangent space point is used for converting points on a manifold to coordinates,
+    - Originally written just for Lie Groups to support legacy, but future needs may well alter the design.
+- Original design driven by parametric solve and dead reckon tethering.
+
+See also: [`accumulateFactorMeans`](@ref), [`solveFactorParameteric`](@ref)
 """
 function getMeasurementParametric end
 
 function getMeasurementParametric(Z)
   error("$(typeof(Z)) is not supported, please use non-parametric or open an issue if it should be")
 end
 
-function getMeasurementParametric(M::AbstractManifold, Z::Normal)
+function getMeasurementParametric(Z::Normal)
   meas = mean(Z)
   iσ = 1/std(Z)^2
   return [meas], reshape([iσ],1,1)
 end
 
-function getMeasurementParametric(M::AbstractManifold, Z::MvNormal)
+function getMeasurementParametric(Z::MvNormal)
   meas = mean(Z)
   iΣ = invcov(Z)
-  p_μ = exp(M, identity_element(M), hat(M, identity_element(M), meas))
-  return p_μ, iΣ
+  return meas, iΣ
 end
 
-getMeasurementParametric(Z::Normal)   = getMeasurementParametric(TranslationGroup(1), Z)
-getMeasurementParametric(Z::MvNormal) = getMeasurementParametric(TranslationGroup(length(mean(Z))), Z)
-
 # the point `p` on the manifold is the mean
 function getMeasurementParametric(s::ManifoldPrior)
   meas = s.p
@@ -87,6 +90,7 @@ function getMeasurementParametric(s::AbstractFactor)
   else
     error("$(typeof(s)) not supported, please use non-parametric or open an issue if it should be")
   end
+  
   return getMeasurementParametric(Z)
 end
 
@@ -114,6 +118,7 @@ function CalcFactorMahalanobis(fct::DFGFactor)
   multihypo = getSolverData(fct).multihypo
   nullhypo = getSolverData(fct).nullhypo
 
+  # FIXME, type instability, use dispatch instead of if-else
   if length(multihypo) > 0
     special = MaxMultihypo(multihypo)
   elseif nullhypo > 0 

src/SolverUtilities.jl

@@ -9,14 +9,16 @@ function fastnorm(u)
   @fastmath @inbounds return sqrt(s)
 end
 
+
 """
     $SIGNATURES
 
 Helper function for IIF.getSample, to generate a vector of points regardless of the Manifold and probability density function used.
 
-Related
+DevNotes:
+- FIXME deprecate to `sample`, see JuliaRobotics/RoME.jl#465
 
-[`getSample`](@ref)
+See also: [`getSample`](@ref), [`rand`]
 """
 randToPoints(distr::SamplableBelief, N::Int=1) = [rand(distr,1)[:] for i in 1:N]
 randToPoints(distr::ManifoldKernelDensity, N::Int=1) = rand(distr,N)
@@ -216,10 +218,10 @@ Related
 [`RoME.generateCanonicalFG_Honeycomb!`](@ref), [`accumulateFactorMeans`](@ref), [`getPPE`](@ref)
 """
 function _checkVariableByReference( fg::AbstractDFG,
-                                    srcLabel::Symbol,            # = :x5
-                                    destRegex::Regex,            # = r"l\d+"
-                                    destType::Type{<:InferenceVariable}, # = Point2
-                                    factor::AbstractRelative;    # = Pose2Poin2BearingRange(...)
+                                    srcLabel::Symbol,
+                                    destRegex::Regex,
+                                    destType::Type{<:InferenceVariable},
+                                    factor::AbstractRelative;
                                     srcType::Type{<:InferenceVariable} = getVariableType(fg, srcLabel) |> typeof,
                                     refKey::Symbol=:simulated,
                                     prior = DFG._getPriorType(srcType)( MvNormal(getPPE(fg[srcLabel], refKey).suggested, diagm(ones(getDimension(srcType)))) ),
@@ -245,16 +247,13 @@ function _checkVariableByReference( fg::AbstractDFG,
   end
 
   ppe = DFG.MeanMaxPPE(refKey, refVal, refVal, refVal)
-  # setPPE!(v_n, refKey, DFG.MeanMaxPPE, ppe)
 
   # now check if we already have a landmark at this location
   varLms = ls(fg, destRegex) |> sortDFG
   ppeLms = getPPE.(getVariable.(fg, varLms), refKey) .|> x->x.suggested
-  # @show typeof(ppeLms)
   errmask = ppeLms .|> x -> norm(x - ppe.suggested) < atol
   already = any(errmask)
 
-  # @assert sum(errmask) <= 1 "There should be only one landmark at $ppe"
   if already
     # does exist, ppe, variableLabel
     alrLm = varLms[findfirst(errmask)]
@@ -267,9 +266,9 @@ end
 
 
 function _checkVariableByReference( fg::AbstractDFG,
-                                    srcLabel::Symbol,            # = :x5
-                                    destRegex::Regex,            # = r"l\d+"
-                                    destType::Type{<:InferenceVariable}, # = Point2
+                                    srcLabel::Symbol,
+                                    destRegex::Regex,
+                                    destType::Type{<:InferenceVariable},
                                     factor::AbstractPrior;
                                     srcType::Type{<:InferenceVariable} = getVariableType(fg, srcLabel) |> typeof,
                                     refKey::Symbol=:simulated,
@@ -293,4 +292,7 @@ function _checkVariableByReference( fg::AbstractDFG,
 end
 
 
-#
+
+
+
+#

src/entities/OptionalDensities.jl

@@ -31,7 +31,7 @@ Notes
 - Sampling can be more nuanced by injecting a hint, or location of interest:
   - Mostly aimed at limiting compute when faced with massive heatmaps, e.g. nav units are 10's but map is ~1e6.
 - Density approximation is constructed on Guassian measurement assumption of level set and sigma variation.
-- Assume data is on a regular grid on Euclidean(2)
+- Assume data is on a regular grid on TranslationGroup(2)
 
 DevNotes:
 - Generalize to scalar fields on any Manifold.
@@ -54,7 +54,7 @@ struct HeatmapDensityRegular{T <: Real, H <: Union{<:Function, Nothing}, B <: Un
   """general rule for kernel bandwidths used in construction of density, e.g. 0.7 of domain grid spacing"""
   bw_factor::T 
   """density function as samplable representation of the data over the domain"""
-  densityFnc::B # TODO change to ::ManifoldKernelDensity{Euclidean(2),BallTreeDensity}
+  densityFnc::B # TODO change to ::ManifoldKernelDensity{TranslationGroup(2),BallTreeDensity}
 end
 
 (hmd::HeatmapDensityRegular)(w...;kw...) = hmd.densityFnc(w...;kw...)

src/entities/SolverParams.jl

@@ -81,7 +81,7 @@ SolverParams(;dimID::Int=0,
               gibbsIters::Int=3,
               maxincidence::Int=500,
               alwaysFreshMeasurements::Bool=true,
-              attemptGradients::Bool=false,
+              attemptGradients::Bool=true,
               devParams::Dict{Symbol,String}=Dict{Symbol,String}()
             ) = begin useMsgLikelihoods==true && @warn "useMsgLikelihoods is under development, use with care, see #1010"
                 SolverParams( dimID,

src/services/ApproxConv.jl

@@ -244,7 +244,8 @@ end
 """
     $SIGNATURES
 
-Helper function to propagate a parametric estimate along a factor chain.
+Helper function to propagate a parametric estimate along a factor chain.  
+This function takes and returns variable values as coordinates.
 
 Notes
 - Not used during MM-iSAM inference.
@@ -276,25 +277,32 @@ function solveFactorParameteric(dfg::AbstractDFG,
   # get the measurement point
   fctTyp = getFactorType(fct)
   mea, _ = getMeasurementParametric(fctTyp)
+  # should this be coords, tangent, or point
   measT = (mea,)
 
   # get variable points
   function _getParametric(vari::DFGVariable, key=:default)
     # hasp = haskey(getPPEDict(vari), key)
     # FIXME use PPE via Manifold points currently in coordinates
     # hasp ? getPPE(vari, key).suggested : calcMean(getBelief(vari, key))
-    calcMean(getBelief(vari, key))
+    pt = calcMean(getBelief(vari, key))
+
+    getCoordinates(getVariableType(vari),pt)
   end
 
   # overwrite specific src values from user
-  prms = _getParametric.(getVariable.(dfg, varLbls), solveKey)
+  coordVals = _getParametric.(getVariable.(dfg, varLbls), solveKey)
   for (srcsym, currval) in srcsym_vals
-    prms[findfirst(varLbls .== srcsym)] = currval
+    coordVals[findfirst(varLbls .== srcsym)] = currval
   end
-  pts = tuple(prms...)
+  crds = tuple(coordVals...)
 
-  # do the calculation to find solvefor index using the factor
-  return _evalFactorTemporary!( fctTyp, varTypes, sfidx, measT, pts; solveKey=solveKey, evaltmpkw... )
+  pts = tuple(map(t->getPoint(t...), zip(varTypes,crds))...)
+
+  # do the calculation to find solvefor index using the factor, as manifold point
+  pt = _evalFactorTemporary!( fctTyp, varTypes, sfidx, measT, pts; solveKey=solveKey, evaltmpkw... )[1]
+
+  return getCoordinates(getVariableType(dfg, trgsym), pt)
 end