Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Solver is not producing PPEs, probably related to the setup of the problem #1507

Closed
GearsAD opened this issue Mar 27, 2022 · 9 comments
Closed

Solver is not producing PPEs, probably related to the setup of the problem #1507

GearsAD opened this issue Mar 27, 2022 · 9 comments
Assignees
@dehann
Labels
bug manifolds needs testing PPE
Milestone
v0.27.4

Comments

@GearsAD
Copy link
Collaborator

GearsAD commented Mar 27, 2022 •
edited
Loading

Looking for ways to debug this:

I'm seeing this in the solver: [ Info: MethodError(view, (ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}(([2.295244785153452, -2.251804188991782], [-0.30974230750707976 -0.950820541921655; 0.950820541921655 -0.30974230750707976])), [1, 2, 3]), 0x0000000000007d63)

And no PPEs after solving.

My setup is fairly straighforward, although it's through the Python layer so it might look a little different for an IIF setup:

variables = [
    Variable("x0", VariableType.Pose2.value),
    Variable("x1", VariableType.Pose2.value),
    Variable("x2", VariableType.Pose2.value),
    Variable("l0", VariableType.Point2.value),
]
factors = [
    Factor(
        "x0f1",
        "PriorPose2",
        ["x0"],
        FactorData(
            fnc=PriorPose2(
                FullNormal(np.zeros(3), np.diag([0.1, 0.1, 0.1]))
            ).dump()  # This is a generator for a PriorPose2
        ),
    ),
    Factor(
        "x0x1f1",
        "Pose2Pose2",
        ["x0", "x1"],
        FactorData(
            fnc=Pose2Pose2(
                FullNormal([1, 1, np.pi / 3], np.diag([0.1, 0.1, 0.1]))
            ).dump()  # This is a generator for a PriorPose2
        ),
    ),
    Factor(
        "x1x2f1",
        "Pose2Pose2",
        ["x1", "x2"],
        FactorData(
            fnc=Pose2Pose2(
                FullNormal([1, 1, np.pi / 3], np.diag([0.1, 0.1, 0.1]))
            ).dump()  # This is a generator for a PriorPose2
        ),
    ),
    # TODO: Improve problem setup in future.
    Factor(
        "l0f1",
        "PriorPoint2",
        ["l0"],
        FactorData(
            fnc=PriorPoint2(FullNormal(np.asarray([5, 0]), np.diag([2, 2]))).dump()
        ),
    ),
    Factor(
        "x0l0f1",
        "Point2Point2Range",
        ["x0", "l0"],
        FactorData(fnc=Point2Point2Range(Normal(5, 1)).dump()),  # Range
    ),
    Factor(
        "x0l0f2",
        "Pose2Point2BearingRange",
        ["x0", "l0"],
        FactorData(
            fnc=Pose2Point2BearingRange(
                Normal(0, 0.3), Normal(5, 1)  # Bearing, range
            ).dump()
        ),
    ),

Any pointers on how to debug this?
@dehann
Copy link
Member

dehann commented Mar 27, 2022 •
edited
Loading

EDIT: i think the PPE is there but the PPE data is being stored as a point not a coordinate. This might be a legacy bug following the Manifolds upgrade to IIF.

I'm seeing this in the solver: [ Info: MethodError(view, (ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}(([2.295244785153452, -2.251804188991782], [-0.30974230750707976 -0.950820541921655; 0.950820541921655 -0.30974230750707976])), [1, 2, 3]), 0x0000000000007d63)

Any chance of a call stack on this error. It looks like some older code trying to use PPE as a coordinate view(vec, dims), but it's coming up against the newer Manifolds.ProductRepr way of writing points. Think this is in IIF somewhere

The fix will likely be to do something like this:

view( AMP.makeCoordsFromPoint(Mani, ppe),    [1,2,3] )

https://github.com/JuliaRobotics/ApproxManifoldProducts.jl/blob/82c7657e7ff2aa14b106a5fd2fa8d9ebf775b330/src/Interface.jl#L37

not the worlds greatest convert function, but at least we can easily upgrade them all by using the same one from AMP.

Looks like a missed upgrade bug, the difference between coordinates and newer on-Manifold point representation.
For Pose2,

  • data stored as coordinates would be [x,y,theta].
  • data stored as a point in the newer Manifolds.ProductRepr([x;y], RotMatZ(theta))
    • e.g. ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}(([2.295244785153452, -2.251804188991782], [-0.30974230750707976 -0.950820541921655; 0.950820541921655 -0.30974230750707976])

If we can see the call stack line of code (in IIF) where this MethodError occurred, it should be straight forward to just switch the point to coordinates and then pull the dimensions of interest ([1,2,3] in this case).

Once we figured this out, we should also add this to the IIF tests to make sure it is covered in the future

@dehann dehann added this to the v0.27.4 milestone Mar 27, 2022
@GearsAD
Copy link
Collaborator Author

GearsAD commented Mar 27, 2022

To clarify, I believe the solver is not completing. When pulling back the PPE's, here is what I see:

>>> ppes = {v: (await getVariable(navability_client, client, v)).ppes.get("default", None) for v in vars}
>>> ppes
{'l1': None, 'l0': None, 'x2': None, 'x1': None, 'x0': None}

@GearsAD
Copy link
Collaborator Author

GearsAD commented Mar 27, 2022

Digging a bit, there's definitely solver data but no PPEs:

>>> v.solverData
{'default': <VariableNodeData(variableType=RoME.Pose2, solveKey={self.solveKey}, dims={self.dims})>}
>>> v.ppes
{}
>>> v = await getVariable(navability_client, client, "x0")
>>> v.solverData
{'default': <VariableNodeData(variableType=RoME.Pose2, solveKey={self.solveKey}, dims={self.dims})>}
>>> v.ppes
{}

@GearsAD
Copy link
Collaborator Author

GearsAD commented Mar 27, 2022 •
edited
Loading 

...
| [ Info: [SOLVING] START [User's session]                                                                                                                                                                                                                                                                                      |
| ┌ Warning: Unable to create measurements and gradients for Point2Point2Range{Normal{Float64}}(Normal{Float64}(μ=5.0, σ=1.0)) during prep of CCW, falling back on no-partial information assumption.  Enable ENV["JULIA_DEBUG"] = "IncrementalInference" for @debug printing to see the error.                                                       |
| └ @ IncrementalInference ~/.julia/packages/IncrementalInference/zwqKk/src/services/CalcFactor.jl:405                                                                                                                                                                                                                                                |
| ┌ Warning: Unable to create measurements and gradients for Point2Point2Range{Normal{Float64}}(Normal{Float64}(μ=5.0, σ=1.0)) during prep of CCW, falling back on no-partial information assumption.  Enable ENV["JULIA_DEBUG"] = "IncrementalInference" for @debug printing to see the error.                                                       |
| └ @ IncrementalInference ~/.julia/packages/IncrementalInference/zwqKk/src/services/CalcFactor.jl:405                                                                                                                                                                                                                                                |
| ┌ Warning: Unable to create measurements and gradients for Pose2Pose2{FullNormal}(FullNormal(                                                                                                                                                                                                                                                       |
| │ dim: 3                                                                                                                                                                                                                                                                                                                                            |
| │ μ: [1.0, 1.0, 1.0471975511965976]                                                                                                                                                                                                                                                                                                                 |
| │ Σ: [0.1 0.0 0.0; 0.0 0.1 0.0; 0.0 0.0 0.1]                                                                                                                                                                                                                                                                                                        |
| │ )                                                                                                                                                                                                                                                                                                                                                 |
| │ ) during prep of CCW, falling back on no-partial information assumption.  Enable ENV["JULIA_DEBUG"] = "IncrementalInference" for @debug printing to see the error.                                                                                                                                                                                |
| └ @ IncrementalInference ~/.julia/packages/IncrementalInference/zwqKk/src/services/CalcFactor.jl:405                                                                                                                                                                                                                                                |
| ┌ Warning: Unable to create measurements and gradients for Pose2Pose2{FullNormal}(FullNormal(                                                                                                                                                                                                                                                       |
| │ dim: 3                                                                                                                                                                                                                                                                                                                                            |
| │ μ: [1.0, 1.0, 1.0471975511965976]                                                                                                                                                                                                                                                                                                                 |
| │ Σ: [0.1 0.0 0.0; 0.0 0.1 0.0; 0.0 0.0 0.1]                                                                                                                                                                                                                                                                                                        |
| │ )                                                                                                                                                                                                                                                                                                                                                 |
| │ ) during prep of CCW, falling back on no-partial information assumption.  Enable ENV["JULIA_DEBUG"] = "IncrementalInference" for @debug printing to see the error.                                                                                                                                                                                |
| └ @ IncrementalInference ~/.julia/packages/IncrementalInference/zwqKk/src/services/CalcFactor.jl:405                                                                                                                                                                                                                                                |
| ┌ Warning: Unable to create measurements and gradients for Pose2Pose2{FullNormal}(FullNormal(                                                                                                                                                                                                                                                       |
| │ dim: 3                                                                                                                                                                                                                                                                                                                                            |
| │ μ: [1.0, 1.0, 1.0471975511965976]                                                                                                                                                                                                                                                                                                                 |
| │ Σ: [0.1 0.0 0.0; 0.0 0.1 0.0; 0.0 0.0 0.1]                                                                                                                                                                                                                                                                                                        |
| │ )                                                                                                                                                                                                                                                                                                                                                 |
| │ ) during prep of CCW, falling back on no-partial information assumption.  Enable ENV["JULIA_DEBUG"] = "IncrementalInference" for @debug printing to see the error.                                                                                                                                                                                |
| └ @ IncrementalInference ~/.julia/packages/IncrementalInference/zwqKk/src/services/CalcFactor.jl:405                                                                                                                                                                                                                                                |
| ┌ Warning: Unable to create measurements and gradients for Pose2Pose2{FullNormal}(FullNormal(                                                                                                                                                                                                                                                       |
| │ dim: 3                                                                                                                                                                                                                                                                                                                                            |
| │ μ: [1.0, 1.0, 1.0471975511965976]                                                                                                                                                                                                                                                                                                                 |
| │ Σ: [0.1 0.0 0.0; 0.0 0.1 0.0; 0.0 0.0 0.1]                                                                                                                                                                                                                                                                                                        |
| │ )                                                                                                                                                                                                                                                                                                                                                 |
| │ ) during prep of CCW, falling back on no-partial information assumption.  Enable ENV["JULIA_DEBUG"] = "IncrementalInference" for @debug printing to see the error.                                                                                                                                                                                |
| └ @ IncrementalInference ~/.julia/packages/IncrementalInference/zwqKk/src/services/CalcFactor.jl:405                                                                                                                                                                                                                                                |
| ┌ Warning: Unable to create measurements and gradients for Pose2Point2BearingRange{Normal{Float64}, Normal{Float64}}(Normal{Float64}(μ=0.0, σ=0.3), Normal{Float64}(μ=5.0, σ=1.0)) during prep of CCW, falling back on no-partial information assumption.  Enable ENV["JULIA_DEBUG"] = "IncrementalInference" for @debug printing to see the error. |
| └ @ IncrementalInference ~/.julia/packages/IncrementalInference/zwqKk/src/services/CalcFactor.jl:405                                                                                                                                                                                                                                                |
| ┌ Warning: Unable to create measurements and gradients for Pose2Point2BearingRange{Normal{Float64}, Normal{Float64}}(Normal{Float64}(μ=0.0, σ=0.3), Normal{Float64}(μ=5.0, σ=1.0)) during prep of CCW, falling back on no-partial information assumption.  Enable ENV["JULIA_DEBUG"] = "IncrementalInference" for @debug printing to see the error. |
| └ @ IncrementalInference ~/.julia/packages/IncrementalInference/zwqKk/src/services/CalcFactor.jl:405                                                                                                                                                                                                                                                |
| [ Info: Ensure variables are all initialized (graphinit)                                                                                                                                                                                                                                                                                            |
| [ Info: x1 is not initialized, and will do so now...                                                                                                                                                                                                                                                                                                |
| [ Info: try doautoinit! of x1                                                                                                                                                                                                                                                                                                                       |
| [ Info: init with useinitfct Symbol[]                                                                                                                                                                                                                                                                                                               |
| [ Info: x0 is not initialized, and will do so now...                                                                                                                                                                                                                                                                                                |
| [ Info: try doautoinit! of x0                                                                                                                                                                                                                                                                                                                       |
| [ Info: init with useinitfct [:x0f1]                                                                                                                                                                                                                                                                                                                |
| [ Info: do init of x0                                                                                                                                                                                                                                                                                                                               |
| [ Info: x2 is not initialized, and will do so now...                                                                                                                                                                                                                                                                                                |
| [ Info: try doautoinit! of x2                                                                                                                                                                                                                                                                                                                       |
| [ Info: init with useinitfct Symbol[]                                                                                                                                                                                                                                                                                                               |
| [ Info: l0 is not initialized, and will do so now...                                                                                                                                                                                                                                                                                                |
| [ Info: try doautoinit! of l0                                                                                                                                                                                                                                                                                                                       |
| [ Info: init with useinitfct [:x0l0f1, :l0f1]                                                                                                                                                                                                                                                                                                       |
| [ Info: do init of l0                                                                                                                                                                                                                                                                                                                               |
| [ Info: MethodError(getindex, (ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}(([-0.06222381950265887, -0.6784666285814913], [0.9878938592211083 0.15513130861700686; -0.15513130861700686 0.9878938592211083])), 1:2), 0x0000000000007d63)                                                                                                    |
| [ Info: MethodError(getindex, (ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}(([-0.06222381950265887, -0.6784666285814913], [0.9878938592211083 0.15513130861700686; -0.15513130861700686 0.9878938592211083])), 1:2), 0x0000000000007d63)                                                                                                    |

...

I'm expecting to see an ALL DONE message here. It looks like it's failing on the init of l0 for some reason.

@dehann
Copy link
Member

dehann commented Mar 28, 2022

Alright, I'll work to recreate and fix from info above, thanks!

@dehann dehann self-assigned this Mar 28, 2022
@dehann
Copy link
Member

dehann commented Mar 28, 2022 •
edited
Loading

From Slack, Sam:
"""
Python SDK is ready but the solver is not solving the graph correctly (I am not seeing the results I need to call the test complete). We need that resolved before releasing the Python SDK.

[should write a unit test for future]
"""

@dehann dehann moved this from Todo to In Progress in Code Fixes for ICRA 2022 Mar 28, 2022
@dehann dehann mentioned this issue Mar 28, 2022
Open
dehann added a commit to JuliaRobotics/RoME.jl that referenced this issue Mar 28, 2022
@dehann
new failing test on PPE,
13caaa8 
- JuliaRobotics/IncrementalInference.jl#1507
@dehann
Copy link
Member

dehann commented Mar 28, 2022

Got the error stack with new test file added to RoME:

[ Info: do init of l0
ERROR: MethodError: no method matching getindex(::ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}, ::UnitRange{Int64})
Closest candidates are:
  getindex(::ProductRepr, ::Manifolds.ProductManifold, ::Union{Colon, Integer, Val, AbstractVector}) at ~/.julia/packages/Manifolds/7YSUf/src/manifolds/ProductManifold.jl:708
  getindex(::ProductRepr, ::Manifolds.VectorBundle, ::Symbol) at ~/.julia/packages/Manifolds/7YSUf/src/manifolds/VectorBundle.jl:534
Stacktrace:
  [1] WARNING: both ApproxManifoldProducts and TransformUtils export "rotate!"; uses of it in module RoME must be qualified
(::CalcFactor{Point2Point2Range{Normal{Float64}}, FactorMetadata{SubArray{DFGVariable, 1, Vector{DFGVariable}, Tuple{Vector{Int64}}, false}, SubArray{Symbol, 1, Vector{Symbol}, Tuple{Vector{Int64}}, false}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing}, Vector{Vector{Float64}}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing})(rho::Vector{Float64}, xi::ProductRepr{Tuple{Vector{Float64}, Matrix{Float64}}}, lm::Vector{Float64})
    @ RoME ~/.julia/dev/RoME/src/factors/Range2D.jl:17
  [2] #307
    @ ~/.julia/dev/IncrementalInference/src/NumericalCalculations.jl:255 [inlined]
  [3] (::IncrementalInference.var"#316#317"{SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, IncrementalInference.var"#307#311"{Int64, Vector{Vector{Float64}}, CalcFactor{Point2Point2Range{Normal{Float64}}, FactorMetadata{SubArray{DFGVariable, 1, Vector{DFGVariable}, Tuple{Vector{Int64}}, false}, SubArray{Symbol, 1, Vector{Symbol}, Tuple{Vector{Int64}}, false}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing}, Vector{Vector{Float64}}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}}})(x::Vector{Float64})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/NumericalCalculations.jl:309
  [4] (::IncrementalInference.var"#293#295"{IncrementalInference.var"#316#317"{SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, IncrementalInference.var"#307#311"{Int64, Vector{Vector{Float64}}, CalcFactor{Point2Point2Range{Normal{Float64}}, FactorMetadata{SubArray{DFGVariable, 1, Vector{DFGVariable}, Tuple{Vector{Int64}}, false}, SubArray{Symbol, 1, Vector{Symbol}, Tuple{Vector{Int64}}, false}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing}, Vector{Vector{Float64}}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}}}, Vector{Float64}})(x::Vector{Float64})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/NumericalCalculations.jl:69
  [5] value!!(obj::NLSolversBase.NonDifferentiable{Float64, Vector{Float64}}, x::Vector{Float64})
    @ NLSolversBase ~/.julia/packages/NLSolversBase/cfJrN/src/interface.jl:9
  [6] initial_state(method::Optim.NelderMead{Optim.AffineSimplexer, Optim.AdaptiveParameters}, options::Optim.Options{Float64, Nothing}, d::NLSolversBase.NonDifferentiable{Float64, Vector{Float64}}, initial_x::Vector{Float64})
    @ Optim ~/.julia/packages/Optim/wFOeG/src/multivariate/solvers/zeroth_order/nelder_mead.jl:171
  [7] optimize(d::NLSolversBase.NonDifferentiable{Float64, Vector{Float64}}, initial_x::Vector{Float64}, method::Optim.NelderMead{Optim.AffineSimplexer, Optim.AdaptiveParameters}, options::Optim.Options{Float64, Nothing})
    @ Optim ~/.julia/packages/Optim/wFOeG/src/multivariate/optimize/optimize.jl:36
  [8] optimize(f::Function, initial_x::Vector{Float64}; inplace::Bool, autodiff::Symbol, kwargs::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})
    @ Optim ~/.julia/packages/Optim/wFOeG/src/multivariate/optimize/interface.jl:90
  [9] optimize
    @ ~/.julia/packages/Optim/wFOeG/src/multivariate/optimize/interface.jl:84 [inlined]
 [10] _solveLambdaNumeric(fcttype::Point2Point2Range{Normal{Float64}}, objResX::IncrementalInference.var"#316#317"{SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, IncrementalInference.var"#307#311"{Int64, Vector{Vector{Float64}}, CalcFactor{Point2Point2Range{Normal{Float64}}, FactorMetadata{SubArray{DFGVariable, 1, Vector{DFGVariable}, Tuple{Vector{Int64}}, false}, SubArray{Symbol, 1, Vector{Symbol}, Tuple{Vector{Int64}}, false}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing}, Vector{Vector{Float64}}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}}}, residual::Vector{Float64}, u0::Vector{Float64}, islen1::Bool)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/NumericalCalculations.jl:69
 [11] _solveCCWNumeric!(ccwl::CommonConvWrapper{Point2Point2Range{Normal{Float64}}, Nothing, Vector{Int64}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing, Vector{Float64}, Nothing}; perturb::Float64, _slack::Nothing)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/NumericalCalculations.jl:316
 [12] approxConvOnElements!(ccwl::CommonConvWrapper{Point2Point2Range{Normal{Float64}}, Nothing, Vector{Int64}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing, Vector{Float64}, Nothing}, elements::Vector{Int64}, ::Type{SingleThreaded}, _slack::Nothing)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:40
 [13] approxConvOnElements!(ccwl::CommonConvWrapper{Point2Point2Range{Normal{Float64}}, Nothing, Vector{Int64}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing, Vector{Float64}, Nothing}, elements::Vector{Int64}, _slack::Nothing)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:51
 [14] computeAcrossHypothesis!(ccwl::CommonConvWrapper{Point2Point2Range{Normal{Float64}}, Nothing, Vector{Int64}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing, Vector{Float64}, Nothing}, hyporecipe::NamedTuple{(:certainidx, :allelements, :activehypo, :mhidx), Tuple{UnitRange{Int64}, Vector{Any}, Vector{Any}, Vector{Int64}}}, sfidx::Int64, maxlen::Int64, mani::TranslationGroup{Tuple{2}, ℝ}; spreadNH::Float64, inflateCycles::Int64, skipSolve::Bool, testshuffle::Bool, _slack::Nothing)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:194
 [15] evalPotentialSpecific(Xi::Vector{DFGVariable}, ccwl::CommonConvWrapper{Point2Point2Range{Normal{Float64}}, Nothing, Vector{Int64}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing, Vector{Float64}, Nothing}, solvefor::Symbol, T_::Type{Point2Point2Range{Normal{Float64}}}, measurement::Vector{Tuple}; needFreshMeasurements::Bool, solveKey::Symbol, N::Int64, spreadNH::Float64, inflateCycles::Int64, dbg::Bool, skipSolve::Bool, _slack::Nothing)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:341
 [16] #evalPotentialSpecific#342
    @ ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:504 [inlined]
 [17] evalFactor(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, fct::DFGFactor{CommonConvWrapper{Point2Point2Range{Normal{Float64}}, Nothing, Vector{Int64}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing, Vector{Float64}, Nothing}, 2}, solvefor::Symbol, measurement::Vector{Tuple}; needFreshMeasurements::Bool, solveKey::Symbol, N::Int64, inflateCycles::Int64, dbg::Bool, skipSolve::Bool, _slack::Nothing)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:542
 [18] approxConvBelief(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, fc::DFGFactor{CommonConvWrapper{Point2Point2Range{Normal{Float64}}, Nothing, Vector{Int64}, NamedTuple{(:x0, :l0), Tuple{Vector{Any}, Vector{Any}}}, Nothing, Vector{Float64}, Nothing}, 2}, target::Symbol, measurement::Vector{Tuple}; solveKey::Symbol, N::Int64, skipSolve::Bool)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/ApproxConv.jl:17
 [19] #calcProposalBelief#352
    @ ~/.julia/dev/IncrementalInference/src/services/ApproxConv.jl:161 [inlined]
 [20] proposalbeliefs!(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, destlbl::Symbol, factors::Vector{DFGFactor}, dens::Vector{ManifoldKernelDensity}, measurement::Vector{Tuple}; solveKey::Symbol, N::Int64, dbg::Bool)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/ApproxConv.jl:229
 [21] propagateBelief(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, destvar::DFGVariable{Point2}, factors::Vector{DFGFactor}; solveKey::Symbol, dens::Vector{ManifoldKernelDensity}, N::Int64, needFreshMeasurements::Bool, dbg::Bool, logger::Logging.ConsoleLogger)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/GraphProductOperations.jl:30
 [22] doautoinit!(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, xi::DFGVariable{Point2}; solveKey::Symbol, singles::Bool, N::Int64, logger::Logging.ConsoleLogger)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/GraphInit.jl:126
 [23] doautoinit!(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, Xi::Vector{DFGVariable}; solveKey::Symbol, singles::Bool, N::Int64, logger::Logging.ConsoleLogger)
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/GraphInit.jl:160
 [24] addFactor!(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, Xi::Vector{DFGVariable}, usrfnc::Point2Point2Range{Normal{Float64}}; 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:704
 [25] addFactor!(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, xisyms::Vector{Symbol}, usrfnc::Point2Point2Range{Normal{Float64}}; suppressChecks::Bool, kw::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/FactorGraph.jl:730
 [26] addFactor!(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, xisyms::Vector{Symbol}, usrfnc::Point2Point2Range{Normal{Float64}})
    @ IncrementalInference ~/.julia/dev/IncrementalInference/src/FactorGraph.jl:724
 [27] top-level scope
    @ ~/.julia/dev/RoME/test/testPPE_IIF1507.jl:22

@dehann
Copy link
Member

dehann commented Mar 28, 2022 •
edited
Loading

Oops, think this is a 5 cent problem (but we need a catch warning for this kind of thing):

Should be a Pose2Point2Range, not Point2Point2Range.

    Factor(
        "x0l0f1",
        "Point2Point2Range",
        ["x0", "l0"],
        FactorData(fnc=Point2Point2Range(Normal(5, 1)).dump()),  # Range
    ),

This also touches on the need to solve a growing shortcoming of the code base (aka Standardized Partials):

I think it will be cool if in a case like this, there is an automatic down projection from Pose2 to a Point2 happen, and default is to print a warning that automatic down projections were done. That way this issue would have produced warnings and not held up the show during rapid development cycles.

EDIT: added this to high level design requirements wiki: https://github.com/JuliaRobotics/Caesar.jl/wiki/High-Level-Requirements

@dehann dehann mentioned this issue Mar 28, 2022
Open
6 tasks
@GearsAD
Copy link
Collaborator Author

GearsAD commented Mar 29, 2022

Great, fixed it for me - thanks!

Yes I'd like to know this happened, can you please add stories for an explicit error on this.

@GearsAD GearsAD closed this as completed Mar 29, 2022
Repository owner moved this from In Progress to Done in Code Fixes for ICRA 2022 Mar 29, 2022
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees

@dehann dehann

Labels
bug manifolds needs testing PPE
Projects
No open projects
Code Fixes for ICRA 2022
Status: Done
Milestone
v0.27.4
Development

No branches or pull requests
2 participants
@dehann @GearsAD