Error using Pose2Point2 Factor

[lemauee]

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]

see backport v0.12.1 for fix

[dehann]

Thanks!

[lemauee]

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:

Messed up at Pose 150:

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]

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.