Review how partial Mani Dims/Coords are specified

[dehann]

• Q1) Do we really want to define partial information with a ::Tuple of Ints?
  • e.g. https://github.com/JuliaRobotics/RoME.jl/pull/495/files#diff-84670ed9329bef2d159cd9157c72a49e217e3bad0bb19c83a391aab939b06059R90
• Q2) How do we preserve the correct group actions etc. when "embedding" (marginalizing down) to a partial:
  • First pass: https://github.com/JuliaRobotics/ApproxManifoldProducts.jl/blob/cb410e5974d3b2aba911408f60c8811e31c262b3/src/services/ManifoldPartials.jl#L115-L164

[Affie]

When it comes to partial priors, we currently have one of 2 approaches:

1. Find partial manifold with getManifoldPartial
2. Use the full manifold but partial is done with sampling

#FIXME check if getManifold is defined otherwise fall back to getManifoldPartial, JT: I would like to standardize to getManifold
if hasmethod(getManifold, (typeof(fnc),))
  Msrc = getManifold(fnc)
  setPointPartial!(mani, addEntr[m], Msrc, ccwl.measurement[m][1], partialCoords, false)
else
  Msrc, = getManifoldPartial(mani,partialCoords)
  setPointPartial!(mani, addEntr[m], Msrc, ccwl.measurement[m][1], partialCoords)
end

https://github.com/JuliaRobotics/IncrementalInference.jl/blob/bc4a731c9b9d5c196022e96916b30de2ab323db7/src/services/EvalFactor.jl#L442-L449

[Affie]

We can also standardize with something like: (WIP: I'll add to IIF at some stage)

function samplePointPartial(M::AbstractGroupManifold,
                            z::Distribution,
                            partial::Vector{Int}, 
                            p=identity_element(M), 
                            retraction_method::AbstractRetractionMethod=ExponentialRetraction())
  dim = manifold_dimension(M)
  Xc = zeros(dim)
  Xc[partial] .= rand(z)
  X =  hat(M, p, Xc)
  return retract(M, p, X, retraction_method)
end

struct ManifoldPriorPartial{M <: AbstractManifold, T <: SamplableBelief, P <: Tuple} <: AbstractPrior
    M::M
    Z::T
    partial::P
end 
  
DFG.getManifold(f::ManifoldPriorPartial) = f.M

function getSample(cf::CalcFactor{<:ManifoldPriorPartial}) 
    Z = cf.factor.Z
    M = getManifold(cf.factor)
    partial = collect(cf.factor.partial)

    return (samplePointPartial(M, Z, partial), )
end

[dehann]

Cool, some tweaking once we answer Q1

[Affie]

It might be nice to have the ability to add priors from any submanifold also work correctly. ie. add Translation(3) to SE(3)