dev issue RoME tests with manopt

[dehann]

since mergining:

• Refactor Parametric solve for better performance and better use of Manopt.jl #1778

On using RoME

ERROR: [ Info: monitorCSMs: all tasks done
LoadError: type CalcFactorResidual has no field manifold
Stacktrace:
  [1] getproperty
    @ Base ./Base.jl:37 [inlined]
  [2] (::IncrementalInference.CalcFactorResidual{RoME.PriorPose2{Distributions.FullNormal}, Nothing, 3, 9, SubArray{Float64, 1, RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}, Tuple{Vector{Int64}}, false}, RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}, 1})(m::RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}, p::RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}})
    @ RoME ~/.julia/dev/RoME/src/factors/PriorPose2.jl:41
  [3] (::IncrementalInference.CalcFactorResidual{RoME.PriorPose2{Distributions.FullNormal}, Nothing, 3, 9, SubArray{Float64, 1, RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}, Tuple{Vector{Int64}}, false}, RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}, 1})(p::RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:76
  [4] (::IncrementalInference.var"#546#547"{RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}})(f::IncrementalInference.CalcFactorResidual{RoME.PriorPose2{Distributions.FullNormal}, Nothing, 3, 9, SubArray{Float64, 1, RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}, Tuple{Vector{Int64}}, false}, RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}, 1})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:172
  [5] iterate
    @ Base ./generator.jl:47 [inlined]
  [6] _collect(c::Vector{IncrementalInference.CalcFactorResidual}, itr::Base.Generator{Vector{IncrementalInference.CalcFactorResidual}, IncrementalInference.var"#546#547"{RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}}}, ::Base.EltypeUnknown, isz::Base.HasShape{1})
    @ Base ./array.jl:852
  [7] collect_similar
    @ Base ./array.jl:761 [inlined]
  [8] map
    @ Base ./abstractarray.jl:3275 [inlined]
  [9] IncrementalInference.JacF_RLM!(M::Manifolds.ProductManifold{ℝ, Tuple{IncrementalInference.NPowerManifold{ℝ, Manifolds.GroupManifold{ℝ, Manifolds.ProductManifold{ℝ, Tuple{Manifolds.TranslationGroup{Tuple{2}, ℝ}, Manifolds.SpecialOrthogonal{2}}}, Manifolds.SemidirectProductOperation{Manifolds.RotationAction{Manifolds.TranslationGroup{Tuple{2}, ℝ}, Manifolds.SpecialOrthogonal{2}, Manifolds.LeftForwardAction}}}}, IncrementalInference.NPowerManifold{ℝ, Manifolds.TranslationGroup{Tuple{2}, ℝ}}}}, costF!::IncrementalInference.CostFres!{Tuple{Vector{IncrementalInference.CalcFactorResidual{RoME.PriorPose2{Distributions.FullNormal}, Nothing, 3, 9, SubArray{Float64, 1, RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}, Tuple{Vector{Int64}}, false}, RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}, 1}}, Vector{IncrementalInference.CalcFactorResidual{RoME.Pose2Pose2{Distributions.FullNormal}, Tuple{Manifolds.GroupManifold{ℝ, Manifolds.ProductManifold{ℝ, Tuple{Manifolds.TranslationGroup{Tuple{2}, ℝ}, Manifolds.SpecialOrthogonal{2}}}, Manifolds.SemidirectProductOperation{Manifolds.RotationAction{Manifolds.TranslationGroup{Tuple{2}, ℝ}, Manifolds.SpecialOrthogonal{2}, Manifolds.LeftForwardAction}}}, RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}, Vector{Float64}, RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, 3, 9, SubArray{Float64, 1, RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}, Tuple{Vector{Int64}}, false}, RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}, 2}}, Vector{IncrementalInference.CalcFactorResidual{RoME.Pose2Point2BearingRange{Distributions.Normal{Float64}, Distributions.Normal{Float64}}, Nothing, 2, 4, SubArray{Float64, 1, RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}, Tuple{Vector{Int64}}, false}, RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SMatrix{2, 2, Float64, 4}, StaticArraysCore.SVector{1, Float64}}}, 2}}}}, p::RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}, fg::DistributedFactorGraphs.GraphsDFGs.GraphsDFG{IncrementalInference.SolverParams, DistributedFactorGraphs.DFGVariable, DistributedFactorGraphs.DFGFactor}; all_points::RecursiveArrayTools.ArrayPartition{Float64, Tuple{Vector{RecursiveArrayTools.ArrayPartition{Float64, Tuple{StaticArraysCore.SVector{2, Float64}, StaticArraysCore.SMatrix{2, 2, Float64, 4}}}}, Vector{StaticArraysCore.SVector{2, Float64}}}}, basis_domain::ManifoldsBase.DefaultOrthogonalBasis{ℝ, ManifoldsBase.TangentSpaceType}, is_sparse::Bool)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:172
 [10] solve_RLM(fg::DistributedFactorGraphs.GraphsDFGs.GraphsDFG{IncrementalInference.SolverParams, DistributedFactorGraphs.DFGVariable, DistributedFactorGraphs.DFGFactor}, varlabels::Vector{Symbol}, faclabels::Vector{Symbol}; is_sparse::Bool, finiteDiffCovariance::Bool, kwargs::@Kwargs{})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:334
 [11] solve_RLM (repeats 2 times)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:305 [inlined]
 [12] solveGraphParametric!(::DistributedFactorGraphs.GraphsDFGs.GraphsDFG{IncrementalInference.SolverParams, DistributedFactorGraphs.DFGVariable, DistributedFactorGraphs.DFGFactor}; init::Bool, solveKey::Symbol, is_sparse::Bool, kwargs::@Kwargs{})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:577
 [13] solveGraphParametric!
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:559 [inlined]
 [14] warmUpSolverJIT(; fg::DistributedFactorGraphs.GraphsDFGs.GraphsDFG{IncrementalInference.SolverParams, DistributedFactorGraphs.DFGVariable, DistributedFactorGraphs.DFGFactor}, drawtree::Bool)
    @ RoME ~/.julia/dev/RoME/src/services/AdditionalUtils.jl:22
 [15] warmUpSolverJIT()
    @ RoME ~/.julia/dev/RoME/src/services/AdditionalUtils.jl:8
 [16] macro expansion
    @ ~/.julia/dev/RoME/src/RoME.jl:139 [inlined]
 [17] macro expansion
    @ ~/.julia/packages/PrecompileTools/kmH5L/src/workloads.jl:78 [inlined]
 [18] top-level scope
    @ ~/.julia/dev/RoME/src/RoME.jl:137
 [19] include
    @ Base ./Base.jl:489 [inlined]
 [20] include_package_for_output(pkg::Base.PkgId, input::String, depot_path::Vector{String}, dl_load_path::Vector{String}, load_path::Vector{String}, concrete_deps::Vector{Pair{Base.PkgId, UInt128}}, source::Nothing)
    @ Base ./loading.jl:2216
 [21] top-level scope
    @ stdin:3
in expression starting at /home/dehann/.julia/dev/RoME/src/RoME.jl:1
in expression starting at stdin:
Stacktrace:
  [1] pkgerror(msg::String)
    @ Pkg.Types /usr/local/share/julia-1.10.0-beta3/share/julia/stdlib/v1.10/Pkg/src/Types.jl:70
  [2] precompile(ctx::Pkg.Types.Context, pkgs::Vector{…}; internal_call::Bool, strict::Bool, warn_loaded::Bool, already_instantiated::Bool, timing::Bool, _from_loading::Bool, kwargs::@Kwargs{…})
    @ Pkg.API /usr/local/share/julia-1.10.0-beta3/share/julia/stdlib/v1.10/Pkg/src/API.jl:1626
  [3] precompile(pkgs::Vector{Pkg.Types.PackageSpec}; io::Base.TTY, kwargs::@Kwargs{_from_loading::Bool})
    @ Pkg.API /usr/local/share/julia-1.10.0-beta3/share/julia/stdlib/v1.10/Pkg/src/API.jl:159
  [4] precompile
    @ Pkg.API /usr/local/share/julia-1.10.0-beta3/share/julia/stdlib/v1.10/Pkg/src/API.jl:147 [inlined]
  [5] #precompile#114
    @ Pkg.API /usr/local/share/julia-1.10.0-beta3/share/julia/stdlib/v1.10/Pkg/src/API.jl:146 [inlined]
  [6] _require(pkg::Base.PkgId, env::String)
    @ Base ./loading.jl:1957
  [7] __require_prelocked(uuidkey::Base.PkgId, env::String)
    @ Base ./loading.jl:1806
  [8] #invoke_in_world#3
    @ Base ./essentials.jl:921 [inlined]
  [9] invoke_in_world
    @ Base ./essentials.jl:918 [inlined]
 [10] _require_prelocked(uuidkey::Base.PkgId, env::String)
    @ Base ./loading.jl:1797
 [11] macro expansion
    @ Base ./loading.jl:1784 [inlined]
 [12] macro expansion
    @ Base ./lock.jl:267 [inlined]
 [13] __require(into::Module, mod::Symbol)
    @ Base ./loading.jl:1747
 [14] #invoke_in_world#3
    @ Base ./essentials.jl:921 [inlined]
 [15] invoke_in_world
    @ Base ./essentials.jl:918 [inlined]
 [16] require(into::Module, mod::Symbol)
    @ Base ./loading.jl:1740
Some type information was truncated. Use `show(err)` to see complete types.

[Affie]

Maybe this should be a design discussion.
I would like factors to call getManifold or hard code the Manifold for now. It used to be like this in most of the factors and I've reverted it where I saw. The reason is to first simplify CalcFactor as much as possible to get rid of dynamic dispatch which is a major bottleneck in performance and then add fields as needed. If done correctly, getManifolds uses constant propagation during compile time and is faster than getting a field in a struct.
also see #1480 (comment)

[Affie]

Regardless, it looks like getManifolds is faster:

julia> @btime getManifold(cf)
  13.105 ns (0 allocations: 0 bytes)
SpecialEuclidean(2)

julia> @btime cf.manifold
  77.283 ns (1 allocation: 544 bytes)
SpecialEuclidean(2)

Here is ideal:

julia> @code_lowered getManifold(Pose2)
CodeInfo(
1 ─ %1 = RoME.SpecialEuclidean(2)
└──      return %1
)

julia> @btime getManifold(Pose2)
  1.540 ns (0 allocations: 0 bytes)
SpecialEuclidean(2)