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

Error using Pose2Point2 Factor #388

Open
lemauee opened this issue Jan 28, 2021 · 4 comments · Fixed by #389
Open

Error using Pose2Point2 Factor #388

lemauee opened this issue Jan 28, 2021 · 4 comments · Fixed by #389
Labels
bug factors needs testing
Milestone
v0.0.x

Comments

@lemauee
Copy link
Contributor

lemauee commented Jan 28, 2021

Hi,

For comparison to the Pose2Point2BearingRange factor, I wanted to try the Pose2Point2 factor on my well-known example:

## Read Gt Input from files

using CSV

associationLm = CSV.File("association_lm.txt", header=["iPose", "jLm"])
gtLm = CSV.File("gt_lm.txt", header=["x", "y", "θ"])
gtTraj = CSV.File("gt_traj.txt", header=["x", "y", "θ"])

## Generate "xy" measurements from groundtruth

using CoordinateTransformations, TransformUtils, LinearAlgebra

Tinv(x,y,θ) = inv(Translation(x,y) ∘ LinearMap(R(θ)))

mTrajGt = [Vector{Float64}(undef,3) for _ = 1:length(gtTraj)]
mLmGt = [Dict{Int,Vector{Float64}}() for _ = 1:length(gtTraj)]

for kPose = 1:length(gtTraj)

    # odometry
    if kPose > 1 # no odemetry for first pose, prior instead
        trans = Tinv(gtTraj[kPose-1].x,gtTraj[kPose-1].y,gtTraj[kPose-1].θ)([gtTraj[kPose].x, gtTraj[kPose].y])
        rot = wrapRad(gtTraj[kPose].θ - gtTraj[kPose-1].θ)
        mTrajGt[kPose] = vcat(trans,rot)
    end

    # landmark measurements
    lmIdx = associationLm.jLm[findall(associationLm.iPose .== kPose)]
    for kLm = lmIdx
        mLmGt[kPose][kLm] = Tinv(gtTraj[kPose].x,gtTraj[kPose].y,gtTraj[kPose].θ)([gtLm[kLm].x, gtLm[kLm].y])
    end
end

## Add noise to ground truth measurements

using Distributions, Random

Random.seed!(42); # reproducibility

ΣOdo = Diagonal([0.01,0.01,0.5*pi/180].^2)
ΣLm = Diagonal([0.01,0.01].^2)
# ΣOdo = Diagonal([0.1,0.1,5*pi/180].^2)
# ΣLm = Diagonal([0.1,0.1].^2)

noiseOdo = MvNormal([0,0,0],ΣOdo)
noiseLm = MvNormal([0,0],ΣLm)

mTrajNoisy = [Vector{Float64}(undef,3) for _ = 1:length(gtTraj)]
mLmNoisy = [Dict{Int,Vector{Float64}}() for _ = 1:length(gtTraj)]

for kPose = 1:length(gtTraj)

    # odometry
    if kPose > 1 # no odometry for first pose, prior instead
        mTrajNoisy[kPose] = mTrajGt[kPose] + vec(rand(noiseOdo,1))
    end

    # landmark measurements
    for kLm = keys(mLmGt[kPose])
        mLmNoisy[kPose][kLm] = mLmGt[kPose][kLm] + vec(rand(noiseLm,1))
    end
end

## Build fg from measurements and solve incrementally

using RoME, RoMEPlotting
@everywhere using RoME, RoMEPlotting

endPose = 150;
fg = LightDFG{SolverParams}(solverParams=SolverParams(logpath="br_issue_pose2point2_small_noise"))
getSolverParams(fg).useMsgLikelihoods = false

function addVarIfMissing!(fg,symbol,type,tags)
    if (symbol in ls(fg)) == false
        @info "adding missing variable $(symbol)"
        addVariable!(fg, symbol, type, tags = tags)
    end
end

for kPose = 1:endPose
    
    global fg,tree,smt,hist

    # odometry
    if kPose > 1 # no odometry for first pose
        fromPose = Symbol("x", kPose-1)
        toPose = Symbol("x", kPose)
        addVarIfMissing!(fg, fromPose, Pose2, [:POSE])
        addVarIfMissing!(fg, toPose, Pose2, [:POSE])   
        addFactor!(fg, [fromPose, toPose], Pose2Pose2(MvNormal(mTrajNoisy[kPose], ΣOdo)), graphinit = false)
        initManual!(fg, toPose, [Symbol("x",kPose-1,"x",kPose,"f",1)]) # doing this because I initlialize the same way in GTSAM
    else # prior instead
        addVariable!(fg, :x1, Pose2)
        addFactor!(fg, [:x1], PriorPose2(MvNormal([gtTraj[1].x, gtTraj[1].y, gtTraj[1].θ], Diagonal([0.01,0.01,0.5*pi/180].^2))))
    end

    # landmark measurements
    fromPose = Symbol("x", kPose)
    for kLm = collect(keys(mLmGt[kPose]))
        toLm = Symbol("l", kLm)
        addVarIfMissing!(fg, fromPose, Pose2, [:POSE])
        addVarIfMissing!(fg, toLm, Point2, [:LANDMARK])
        addFactor!(fg, [fromPose, toLm], Pose2Point2(MvNormal(mLmNoisy[kPose][kLm], ΣLm)))
    end

    # solve
    if kPose > 1 # not on first solve
        tree, smt, hist = solveTree!(fg, tree)
    else # first solve
        tree, smt, hist = solveTree!(fg)
    end

    # save
    prefix = lpad(kPose,5,'0')
    saveDFG(fg, "$(getLogPath(fg))/$(prefix)_fg")

    # plot
    pl = plotSLAM2D(fg;contour=false)
    push!(pl, Coord.cartesian(fixed=true),style(background_color=RGB(1,1,1)))
    @async draw(PNG("$(getLogPath(fg))/$(prefix)_poses_landms.png", 20cm, 20cm), pl)

end

unfortunately, this yields a pretty deep-hidden error:

[ Info: adding missing variable x4
[ Info: initManual! x4
[ Info: adding missing variable l17
[ Info: monitorCSMs: all tasks done
[ Info: try doautoinit! of l17
[ Info: Ensure variables are all initialized (graphinit)
[ Info: l17 is not initialized, and will do so now...
[ Info: try doautoinit! of l17
[ Info: init with useinitfct [:x4l17f1]
[ Info: do init of l17
ERROR: LoadError: BoundsError: attempt to access 2-element Array{Float64,1} at index [3]
Stacktrace:
 [1] getindex at ./array.jl:809 [inlined]
 [2] SE2(::Array{Float64,1}) at /home/leopold/.julia/packages/TransformUtils/cFZOw/src/TransformUtils.jl:843
 [3] (::CalcFactor{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}},FactorMetadata{SubArray{DFGVariable,1,Array{DFGVariable,1},Tuple{UnitRange{Int64}},true},SubArray{Symbol,1,Array{Symbol,1},Tuple{UnitRange{Int64}},true},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},Nothing},Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true}})(::Array{Float64,1}, ::SubArray{Float64,1,Array{Float64,2},Tuple{Base.Slice{Base.OneTo{Int64}},Int64},true}, ::SubArray{Float64,1,Array{Float64,2},Tuple{Base.Slice{Base.OneTo{Int64}},Int64},true}, ::SubArray{Float64,1,Array{Float64,2},Tuple{Base.Slice{Base.OneTo{Int64}},Int64},true}) at /home/leopold/.julia/packages/RoME/y7MMZ/src/factors/Pose2Point2.jl:36
 [4] (::IncrementalInference.var"#194#195"{Int64,Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},CalcFactor{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}},FactorMetadata{SubArray{DFGVariable,1,Array{DFGVariable,1},Tuple{UnitRange{Int64}},true},SubArray{Symbol,1,Array{Symbol,1},Tuple{UnitRange{Int64}},true},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},Nothing},Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true}}})(::Array{Float64,1}) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/NumericalCalculations.jl:150
 [5] (::IncrementalInference.var"#197#198"{IncrementalInference.var"#194#195"{Int64,Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},CalcFactor{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}},FactorMetadata{SubArray{DFGVariable,1,Array{DFGVariable,1},Tuple{UnitRange{Int64}},true},SubArray{Symbol,1,Array{Symbol,1},Tuple{UnitRange{Int64}},true},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},Nothing},Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true}}},SubArray{Float64,1,Array{Float64,2},Tuple{Array{Int64,1},Int64},false}})(::Array{Float64,1}, ::Array{Float64,1}) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/NumericalCalculations.jl:194
 [6] (::IncrementalInference.var"#191#193"{IncrementalInference.var"#197#198"{IncrementalInference.var"#194#195"{Int64,Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},CalcFactor{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}},FactorMetadata{SubArray{DFGVariable,1,Array{DFGVariable,1},Tuple{UnitRange{Int64}},true},SubArray{Symbol,1,Array{Symbol,1},Tuple{UnitRange{Int64}},true},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},Nothing},Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true}}},SubArray{Float64,1,Array{Float64,2},Tuple{Array{Int64,1},Int64},false}},Array{Float64,1}})(::Array{Float64,1}) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/NumericalCalculations.jl:62
 [7] value!!(::NLSolversBase.NonDifferentiable{Float64,Array{Float64,1}}, ::Array{Float64,1}) at /home/leopold/.julia/packages/NLSolversBase/QPnui/src/interface.jl:9
 [8] initial_state(::Optim.NelderMead{Optim.AffineSimplexer,Optim.AdaptiveParameters}, ::Optim.Options{Float64,Nothing}, ::NLSolversBase.NonDifferentiable{Float64,Array{Float64,1}}, ::Array{Float64,1}) at /home/leopold/.julia/packages/Optim/Yd5Zq/src/multivariate/solvers/zeroth_order/nelder_mead.jl:171
 [9] optimize(::NLSolversBase.NonDifferentiable{Float64,Array{Float64,1}}, ::Array{Float64,1}, ::Optim.NelderMead{Optim.AffineSimplexer,Optim.AdaptiveParameters}, ::Optim.Options{Float64,Nothing}) at /home/leopold/.julia/packages/Optim/Yd5Zq/src/multivariate/optimize/optimize.jl:33
 [10] optimize(::Function, ::Array{Float64,1}; inplace::Bool, autodiff::Symbol, kwargs::Base.Iterators.Pairs{Union{},Union{},Tuple{},NamedTuple{(),Tuple{}}}) at /home/leopold/.julia/packages/Optim/Yd5Zq/src/multivariate/optimize/interface.jl:90
 [11] optimize at /home/leopold/.julia/packages/Optim/Yd5Zq/src/multivariate/optimize/interface.jl:84 [inlined]
 [12] _solveLambdaNumeric(::Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}, ::IncrementalInference.var"#197#198"{IncrementalInference.var"#194#195"{Int64,Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},CalcFactor{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}},FactorMetadata{SubArray{DFGVariable,1,Array{DFGVariable,1},Tuple{UnitRange{Int64}},true},SubArray{Symbol,1,Array{Symbol,1},Tuple{UnitRange{Int64}},true},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true},Nothing},Tuple{Array{Float64,2}},SubArray{Array{Float64,2},1,Array{Array{Float64,2},1},Tuple{UnitRange{Int64}},true}}},SubArray{Float64,1,Array{Float64,2},Tuple{Array{Int64,1},Int64},false}}, ::Array{Float64,1}, ::Array{Float64,1}; islen1::Bool) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/NumericalCalculations.jl:62
 [13] _solveCCWNumeric!(::CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}}; perturb::Float64, testshuffle::Bool) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/NumericalCalculations.jl:203
 [14] _solveCCWNumeric!(::CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}}) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/NumericalCalculations.jl:182
 [15] approxConvOnElements!(::CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}}, ::Array{Int64,1}, ::Type{SingleThreaded}) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/ApproxConv.jl:68
 [16] approxConvOnElements!(::CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}}, ::Array{Int64,1}) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/ApproxConv.jl:78
 [17] computeAcrossHypothesis!(::CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}}, ::Array{Any,1}, ::Array{Any,1}, ::Array{Int64,1}, ::Int64, ::Int64, ::Tuple{typeof(+),typeof(+)}; spreadNH::Float64) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/ApproxConv.jl:264
 [18] evalPotentialSpecific(::Array{DFGVariable,1}, ::CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}}, ::Symbol, ::Type{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}}, ::Tuple{Array{Float64,2}}; needFreshMeasurements::Bool, solveKey::Symbol, N::Int64, spreadNH::Float64, dbg::Bool) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/ApproxConv.jl:340
 [19] #evalPotentialSpecific#230 at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/ApproxConv.jl:446 [inlined]
 [20] evalFactor(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::DFGFactor{CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}},2}, ::Symbol, ::Tuple{Array{Float64,2}}; needFreshMeasurements::Bool, solveKey::Symbol, N::Int64, dbg::Bool) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/ApproxConv.jl:497
 [21] findRelatedFromPotential(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::DFGFactor{CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}},2}, ::Symbol, ::Tuple{Array{Float64,2}}; N::Int64, solveKey::Symbol, dbg::Bool) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/ApproxConv.jl:712
 [22] proposalbeliefs!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::Symbol, ::Array{DFGFactor{CommonConvWrapper{Pose2Point2{MvNormal{Float64,PDMats.PDiagMat{Float64,Array{Float64,1}},Array{Float64,1}}}},2},1}, ::Array{BallTreeDensity,1}, ::Dict{Int64,Array{BallTreeDensity,1}}, ::Tuple{Array{Float64,2}}; solveKey::Symbol, N::Int64, dbg::Bool) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/GraphProductOperations.jl:168
 [23] #predictbelief#251 at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/GraphProductOperations.jl:213 [inlined]
 [24] predictbelief(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::Symbol, ::Array{Symbol,1}; needFreshMeasurements::Bool, N::Int64, dbg::Bool, logger::Logging.ConsoleLogger) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/GraphProductOperations.jl:237
 [25] doautoinit!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::DFGVariable{Point2}; singles::Bool, N::Int64, logger::Logging.ConsoleLogger) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/FactorGraph.jl:847
 [26] #doautoinit!#56 at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/FactorGraph.jl:877 [inlined]
 [27] ensureAllInitialized!(::LightDFG{SolverParams,DFGVariable,DFGFactor}; solvable::Int64) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/FactorGraph.jl:1054
 [28] ensureAllInitialized! at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/FactorGraph.jl:1039 [inlined]
 [29] solveTree!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::MetaBayesTree; timeout::Nothing, storeOld::Bool, verbose::Bool, verbosefid::Base.TTY, delaycliqs::Array{Symbol,1}, recordcliqs::Array{Symbol,1}, limititercliqs::Array{Pair{Symbol,Int64},1}, injectDelayBefore::Nothing, skipcliqids::Array{Symbol,1}, eliminationOrder::Nothing, variableOrder::Nothing, eliminationConstraints::Array{Symbol,1}, variableConstraints::Nothing, smtasks::Array{Task,1}, dotreedraw::Array{Int64,1}, runtaskmonitor::Bool, algorithm::Symbol, multithread::Bool) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/SolverAPI.jl:307
 [30] solveTree!(::LightDFG{SolverParams,DFGVariable,DFGFactor}, ::MetaBayesTree) at /home/leopold/.julia/packages/IncrementalInference/ShzPg/src/SolverAPI.jl:280
 [31] top-level scope at /home/leopold/Nextcloud/Studium/Masterarbeit/svn/julia/mmiSAM/examples/br_issue/br_issue_pose2point2.jl:108
 [32] include_string(::Function, ::Module, ::String, ::String) at ./loading.jl:1088
 [33] include_string(::Module, ::String, ::String) at ./loading.jl:1096
 [34] invokelatest(::Any, ::Any, ::Vararg{Any,N} where N; kwargs::Base.Iterators.Pairs{Union{},Union{},Tuple{},NamedTuple{(),Tuple{}}}) at ./essentials.jl:710
 [35] invokelatest(::Any, ::Any, ::Vararg{Any,N} where N) at ./essentials.jl:709
 [36] inlineeval(::Module, ::String, ::Int64, ::Int64, ::String; softscope::Bool) at /home/leopold/.vscode-oss/extensions/julialang.language-julia-1.0.10/scripts/packages/VSCodeServer/src/eval.jl:185
 [37] (::VSCodeServer.var"#61#65"{String,Int64,Int64,String,Module,Bool,VSCodeServer.ReplRunCodeRequestParams})() at /home/leopold/.vscode-oss/extensions/julialang.language-julia-1.0.10/scripts/packages/VSCodeServer/src/eval.jl:144
 [38] withpath(::VSCodeServer.var"#61#65"{String,Int64,Int64,String,Module,Bool,VSCodeServer.ReplRunCodeRequestParams}, ::String) at /home/leopold/.vscode-oss/extensions/julialang.language-julia-1.0.10/scripts/packages/VSCodeServer/src/repl.jl:124
 [39] (::VSCodeServer.var"#60#64"{String,Int64,Int64,String,Module,Bool,Bool,VSCodeServer.ReplRunCodeRequestParams})() at /home/leopold/.vscode-oss/extensions/julialang.language-julia-1.0.10/scripts/packages/VSCodeServer/src/eval.jl:142
 [40] hideprompt(::VSCodeServer.var"#60#64"{String,Int64,Int64,String,Module,Bool,Bool,VSCodeServer.ReplRunCodeRequestParams}) at /home/leopold/.vscode-oss/extensions/julialang.language-julia-1.0.10/scripts/packages/VSCodeServer/src/repl.jl:36
 [41] (::VSCodeServer.var"#59#63"{String,Int64,Int64,String,Module,Bool,Bool,VSCodeServer.ReplRunCodeRequestParams})() at /home/leopold/.vscode-oss/extensions/julialang.language-julia-1.0.10/scripts/packages/VSCodeServer/src/eval.jl:110
 [42] with_logstate(::Function, ::Any) at ./logging.jl:408
 [43] with_logger at ./logging.jl:514 [inlined]
 [44] (::VSCodeServer.var"#58#62"{VSCodeServer.ReplRunCodeRequestParams})() at /home/leopold/.vscode-oss/extensions/julialang.language-julia-1.0.10/scripts/packages/VSCodeServer/src/eval.jl:109
 [45] #invokelatest#1 at ./essentials.jl:710 [inlined]
 [46] invokelatest(::Any) at ./essentials.jl:709
 [47] macro expansion at /home/leopold/.vscode-oss/extensions/julialang.language-julia-1.0.10/scripts/packages/VSCodeServer/src/eval.jl:27 [inlined]
 [48] (::VSCodeServer.var"#56#57")() at ./task.jl:356
in expression starting at /home/leopold/Nextcloud/Studium/Masterarbeit/svn/julia/mmiSAM/examples/br_issue/br_issue_pose2point2.jl:80

For the additional files, see #380
@Affie Affie mentioned this issue Jan 29, 2021
Merged
@Affie Affie added this to the v0.12.1 milestone Jan 29, 2021
@Affie
Copy link
Member

Affie commented Jan 29, 2021

see backport v0.12.1 for fix

@Affie Affie closed this as completed Jan 29, 2021
@dehann
Copy link
Member

dehann commented Feb 3, 2021

Thanks!

@lemauee
Copy link
Contributor Author

lemauee commented Feb 3, 2021

Sorry for the late update,

Its running now, but the results are pretty similar to whats going on with the Pose2Point2BearingRange Factor (#380):

Fine up to Pose ~60:
00060_poses_landms

Messed up at Pose 150:
00150_poses_landms

So this now also should be related to JuliaRobotics/IncrementalInference.jl#1010 and JuliaRobotics/IncrementalInference.jl#1051. Maybe opening a new issue for this, renaming and reopening this one or adding this example somwhere else is a good thing to keep track of it.

@dehann
Copy link
Member

dehann commented Feb 7, 2021 •
edited by Affie
Loading

Will investigate thanks, sorry we may have broken something here (it should not be this sensitive). We just completed major internal refactoring of the code in IIF and RoME. I think a common fix option would be to troubleshoot and then resolve JuliaRobotics/IncrementalInference.jl#1051 sooner rather than later.

Whats supposed to happen (as individual steps) is when solving a Point2 from a Pose2 through the factor should give a definitive result:

pp = Pose2Point2(..) 
f1 = addFactor!(fg, [:x1; :l1], pp)

# high to low dim
pts = approxConv(fg, f1.label, :l1)

The reverse should result in approx uniform spread of points in the dimensions that are not constrained by the factor:

pts_ = approxConv(fg, f1.label, :x1)

In this case the theta variable on :x1 should be near uniform.

using RoMEPlotting

# might not be the precise API, im trying to recall without testing JT EDIT: added Pose2()
plotPose(Pose2(), manikde!(pts_, Pose2))

Should see even spread on theta, but x and y should be in line with Point2 x and y according to pp.

@dehann dehann reopened this Feb 7, 2021
@dehann dehann modified the milestones: v0.12.1, v0.0.x Jun 12, 2021
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
bug factors needs testing
Projects
None yet
Milestone
v0.0.x
Development

Successfully merging a pull request may close this issue.

Fix and test Pose2Point2 JuliaRobotics/RoME.jl
3 participants
@dehann @Affie @lemauee