search

JuliaRobotics / RoME.jl

Robot Motion Estimate: Tools, Variables, and Factors for SLAM in robotics; also see Caesar.jl.
MIT License
64 stars 13 forks source link

Error using Pose2Point2 Factor #388

Open lemauee opened 3 years ago

lemauee commented 3 years ago

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 commented 3 years ago

see backport v0.12.1 for fix

dehann commented 3 years ago

Thanks!

lemauee commented 3 years ago

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 commented 3 years ago

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.