Problem with new getSample on Pose2 (assert error in Manifolds)

[dehann]

Fails on this line:

RoME.jl/src/factors/Pose2D.jl

Line 33 in e610c40

X = hat.(Ref(M), Ref(ϵ), Xc)

(i had to deconstruct / remove the Refs for broadcast to get access to the error, so Ln35 error below is the same)

ERROR: LoadError: AssertionError: length(X) == dimN + dimH
Stacktrace:
  [1] get_vector!(G::GroupManifold{ℝ, ProductManifold{ℝ, Tuple{TranslationGroup{Tuple{2}, ℝ}, SpecialOrthogonal{2}}}, Manifolds.SemidirectProductOperation{RotationAction{TranslationGroup{Tuple{2}, ℝ}, SpecialOrthogonal{2}, LeftAction}}}, Y::ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}, p::ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}, X::Vector{Vector{Float64}}, B::ManifoldsBase.VeeOrthogonalBasis{ℝ})
    @ Manifolds ~/.julia/packages/Manifolds/EY0G1/src/groups/semidirect_product_group.jl:174
  [2] get_vector(M::GroupManifold{ℝ, ProductManifold{ℝ, Tuple{TranslationGroup{Tuple{2}, ℝ}, SpecialOrthogonal{2}}}, Manifolds.SemidirectProductOperation{RotationAction{TranslationGroup{Tuple{2}, ℝ}, SpecialOrthogonal{2}, LeftAction}}}, p::ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}, X::Vector{Vector{Float64}}, B::ManifoldsBase.VeeOrthogonalBasis{ℝ})
    @ ManifoldsBase ~/.julia/packages/ManifoldsBase/WepHf/src/bases.jl:582
  [3] get_vector__parent
    @ ~/.julia/packages/ManifoldsBase/WepHf/src/DecoratorManifold.jl:427 [inlined]
  [4] get_vector
    @ ~/.julia/packages/ManifoldsBase/WepHf/src/DecoratorManifold.jl:390 [inlined]
  [5] hat
    @ ~/.julia/packages/ManifoldsBase/WepHf/src/bases.jl:785 [inlined]
  [6] macro expansion
    @ ./show.jl:955 [inlined]
  [7] getSample(cf::CalcFactor{Pose2Pose2{FullNormal}, FactorMetadata{Vector{DFGVariable{Pose2}}, Vector{Symbol}, Vector{Vector{Any}}, Nothing}, Nothing, Vector{Vector{Any}}}, N::Int64)
    @ RoME ~/.julia/dev/RoME/src/factors/Pose2D.jl:35
  [8] sampleFactor(cf::CalcFactor{Pose2Pose2{FullNormal}, FactorMetadata{Vector{DFGVariable{Pose2}}, Vector{Symbol}, Vector{Vector{Any}}, Nothing}, Nothing, Vector{Vector{Any}}}, N::Int64)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/CalcFactor.jl:26
  [9] prepgenericconvolution(Xi::Vector{DFGVariable{Pose2}}, usrfnc::Pose2Pose2{FullNormal}; multihypo::Nothing, nullhypo::Float64, threadmodel::Type, inflation::Float64, _blockRecursion::Bool)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/FactorGraph.jl:596
 [10] getDefaultFactorData(dfg::LightDFG{SolverParams, DFGVariable, DFGFactor}, Xi::Vector{DFGVariable{Pose2}}, usrfnc::Pose2Pose2{FullNormal}; multihypo::Vector{Float64}, nullhypo::Float64, threadmodel::Type, eliminated::Bool, potentialused::Bool, edgeIDs::Vector{Int64}, solveInProgress::Int64, inflation::Float64, _blockRecursion::Bool)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/FactorGraph.jl:682
 [11] addFactor!(dfg::LightDFG{SolverParams, DFGVariable, DFGFactor}, Xi::Vector{DFGVariable{Pose2}}, usrfnc::Pose2Pose2{FullNormal}; multihypo::Vector{Float64}, nullhypo::Float64, solvable::Int64, tags::Vector{Symbol}, timestamp::TimeZones.ZonedDateTime, graphinit::Bool, threadmodel::Type, suppressChecks::Bool, inflation::Float64, namestring::Symbol, _blockRecursion::Bool)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/FactorGraph.jl:759
 [12] addFactor!(dfg::LightDFG{SolverParams, DFGVariable, DFGFactor}, xisyms::Vector{Symbol}, usrfnc::Pose2Pose2{FullNormal}; suppressChecks::Bool, kw::Base.Iterators.Pairs{Symbol, Any, Tuple{Symbol, Symbol, Symbol}, NamedTuple{(:graphinit, :solvable, :tags), Tuple{Bool, Int64, Vector{Symbol}}}})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/FactorGraph.jl:804
 [13] _addPoseCanonical!(fg::LightDFG{SolverParams, DFGVariable, DFGFactor}, prevLabel::Symbol, posecount::Int64, factor::Pose2Pose2{FullNormal}; poseRegex::Regex, genLabel::Symbol, srcType::Type{Pose2}, poseType::Type{Pose2}, graphinit::Bool, solvable::Int64, variableTags::Vector{Symbol}, factorTags::Vector{Symbol}, refKey::Symbol, overridePPE::Vector{Float64}, postpose_cb::typeof(cb))
    @ RoME ~/.julia/dev/RoME/src/canonical/GenerateCommon.jl:44

[Affie]

I think its this #465 (comment)

[dehann]

Really, why is the error in Manifolds though? That would mean the callstack is reporting something incorrect?

[dehann]

just adding a potential xref JuliaManifolds/Manifolds.jl#401

[dehann]

Hi @Affie, unfortunately the comment linked above to 465 is not enough for me to figure out what you mean? getSample for Relative is only used by the residual calculation, and the tests for Hexagonal pass (ie Pose2Pose2), so I think something else is going on here.

[dehann]

see updates in de96e02 and 519022c

[Affie]

Hi, I didn't read the full error stack. The issue linked gives the same error, but for a different reason. So ignore #474 (comment)

[Affie]

I'm not seeing this error, perhaps it is fixed?

[dehann]

Yeah, think it was fixed in #475 and follow-on identity_element... The main thing I'm looking at now is the residual functions expect measurements to come in as tangent vectors, but not all the code is doing that properly yet -- will continue that design discussion at #465