When running the unit tests in the feature/multilangzmq branch, the tests in testpartialpose3.jl are failing due to NaN's in the points of x2.

https://github.com/JuliaRobotics/RoME.jl/blob/feature/multilangzmq/test/testpartialpose3.jl#L53

test PartialPriorRollPitchZ evaluations: Test Failed
  Expression: norm(X2pts[olddims, :] - pts[olddims, :]) < 1.0e-10
   Evaluated: NaN < 1.0e-10
Stacktrace:
 [1] macro expansion at /home/gearsad/.julia/v0.6/RoME/test/testpartialpose3.jl:54 [inlined]
 [2] macro expansion at ./test.jl:860 [inlined]
 [3] anonymous at ./<missing>:?

Although this is to the third decimal place...

xref #60

A test suggests there are 18 allocations within each AMP.difftheta operation.

Trying RoME on raspberry pi 3 Rasbian Stretch with julia 0.61.
Pkg.build succeeds but,
Errors while trying to precompile:
Cairo.jl UndefVarError: _jl_libcairo not defined
Failed to precompile Compose, Gadfly, KernelDensityEstimate

IIF v0.3.7+ will likely have shared memory multithreading for approximate convolutions working -- need to make sure all factor types are compliant.

test fails or succeeds randomly. Must investigate and re-enable.
9fbcb66

@JuliaRegistrator register()

@JuliaRegistrator register()

This is fixed with #64

@JuliaRegistrator register

addFactor!(fg, [:x1;:l1], ...) vs addFactor!(fg, [:l1;:x1], ...)

This could be a landmark too, must standardize.

First off: thank you for all your hard work! I'm so very excited to see more robotics work being done in Julia!

I've been working on a GTSAM-based SLAM module in C++ to enable accurate real-time state estimates for a multi-agent autonomous vehicle research project. I also recently came across Julia, fell in love, and switched all my Python development to Julia.

So, I'm curious if RoME/IncrementalInference is fast and bug-free enough to replace our C++ GTSAM component. As a rough estimate, I would be adding roughly 15-100 factors a second and require state estimates at that same rate.

So, is RoME/IncrementalInference ready for prime time?

Should be, for example:

PriorPose2(Distributions.MvNormal(zeros(3), 0.01*eye(3)))

See recent similar changes to Pose2Pose2:

Pose2Pose2(Distributions.MvNormal(zeros(3), 0.01*eye(3)))

Will do this for RoME v0.1.5 and influences the Hexagonal 2D SLAM examples.

cc @keevindoherty, @GearsAD

So that x,y,z, nx,ny,nz is maintained, similar to Pose3, but allows for working with surfaces in a cleaner way.

cc @pvazteixeira

Travis RoME Build 470.1

test residual function of PartialPose3XYYaw |    5      5
test PartialPose3XYYaw evaluations: Test Failed
  Expression: sum(abs.(Base.mean(pts[newdims, :], 2) - mu2) .< [0.6; 0.6; 0.1]) == 3
   Evaluated: 2 == 3
Stacktrace:
 [1] macro expansion at /home/travis/.julia/v0.6/RoME/test/testpartialpose3.jl:114 [inlined]
 [2] macro expansion at ./test.jl:860 [inlined]
 [3] anonymous at ./<missing>:?
Test Summary:                      | Pass  Fail  Total
test PartialPose3XYYaw evaluations |    5     1      6
ERROR: LoadError: LoadError: Some tests did not pass: 5 passed, 1 failed, 0 errored, 0 broken.
while loading /home/travis/.julia/v0.6/RoME/test/testpartialpose3.jl, in expression starting on line 91
while loading /home/travis/.julia/v0.6/RoME/test/runtests.jl, in expression starting on line 52
================================[ ERROR: RoME ]=================================

@JuliaRegistrator register()

The registry seems to have messed up with the bounds for IIF in compat
Not sure how we want to fix it?

• new tag or
• pr against registry to make it IncrementalInference = "0.7.2-0.8"

Maybe these are related, but they seem like older issues:
JuliaLang/Pkg.jl#1177 (comment)
https://discourse.julialang.org/t/dependency-problem-with-fillarrays/

Hi,

I'm having issues using the addOdoFG! function, mostly because of the AbstractString generic (think that's the term?). I've modified my version to this, which works:

RobotUtils.jl:162 -

function addOdoFG!(
        fg::FactorGraph,
        n::Symbol,
        DX::Array{Float64,1},
        cov::Array{Float64,2};
        N::Int=0,
        ready::Int=1,
        labels::Vector{String}=String[]  )
    #
    prev, X, nextn = getLastPose2D(fg)
    r,c = size(X)
    if N==0
      N = c
    end
    sig = diag(cov)
    RES = zeros(r,c)
    # increases the number of particles based on the number of modes in the measurement Z
    for i in 1:c
        ent = [randn()*sig[1]; randn()*sig[2]; randn()*sig[3]]
        RES[:,i] = addPose2Pose2(X[:,i], DX + ent)
    end

    v = addNode!(fg, n, RES, cov, N=N, ready=ready, labels=labels)
    pp = Pose2Pose2(vectoarr2(DX), cov, [1.0]) #[prev;v],
    f = addFactor!(fg, [prev;v], pp, ready=ready)
    infor = inv(cov^2)
    # addOdoRemote(prev.index,v.index,DX,infor) # this is for remote factor graph ref parametric solution -- skipped internally by global flag variable
    return v, f
end

EDIT: added triple back ticks for function readability

@JuliaRegistrator register branch=release/v0.5

UndefVarError: PackedPose2Point2Bearing not defined when running

p2br = Pose2Point2Bearing(Normal(ψ,0.05))
addFactor([psym; lsym], p2br)

Reported GearsAD/GraffSDK.jl#69, but RoME seems more appropriate.

Drop all 2D for 2 in Point, Pose, Bearing, Range, or other factor and variable types.

@JuliaRegistrator register branch=release/v0.5

Recently added DynPose2, packing types not yet fully tested.

Is this the right model for package dependencies?

IncrementalInference -> RoME -> Caesar -> Arena
|
-> RoMEPlotting?

Related? These warnings:

WARNING: both IncrementalInference and RoME export "compare"; uses of it in module Main must be qualified
WARNING: both IncrementalInference and RoME export "evalPotential"; uses of it in module Main must be qualified

This is at BearingRange2D:23, here:
z = pp2r.Zij[1]+meas[1][1,idx]

Should it be z = pp2r.Z[1]+meas[1][1,idx] ?

Stacktrace:
type Pose2Point2Range has no field Zij
in anonymous at base/
in macro expansion at Sandshark_2bcontinued.jl:163
in addFactor! at IncrementalInference/src/FactorGraph01.jl:634
in #addFactor!#53 at IncrementalInference/src/FactorGraph01.jl:638
in at base/
in #addFactor!#52 at IncrementalInference/src/FactorGraph01.jl:609
in at base/
in #doautoinit!#45 at IncrementalInference/src/FactorGraph01.jl:497
in at base/
in #predictbelief#136 at IncrementalInference/src/SolveTree01.jl:338
in at base/
in #predictbelief#135 at IncrementalInference/src/SolveTree01.jl:313
in at base/
in #proposalbeliefs!#134 at IncrementalInference/src/SolveTree01.jl:282
in findRelatedFromPotential at IncrementalInference/src/ApproxConv.jl:381
in at base/
in #evalFactor2#130 at IncrementalInference/src/ApproxConv.jl:333
in at base/
in #evalPotentialSpecific#127 at IncrementalInference/src/ApproxConv.jl:225
in computeAcrossHypothesis! at IncrementalInference/src/ApproxConv.jl:125
in approxConvOnElements! at IncrementalInference/src/ApproxConv.jl:68
in approxConvOnElements! at IncrementalInference/src/ApproxConv.jl:46
in #numericRootGenericRandomizedFnc!#114 at IncrementalInference/src/SolverUtilities.jl:94
in #nlsolve#39 at NLsolve/src/nlsolve/nlsolve.jl:59
in at base/
in #nlsolve#38 at NLsolve/src/nlsolve/nlsolve.jl:26
in trust_region_ at NLsolve/src/solvers/trust_region.jl:118
in value_jacobian!! at NLSolversBase/src/interface.jl:124

Dyn point is not the most satisfying name, since we may want a point also with accelerations?
DOF is misleading
Have to think about the name

What about ->

struct Point4DOF <: IncrementalInference.InferenceVariable
  dims::Int
  labels::Vector{String}
  Point4() = new(5, String["Point4";]) #x,y,̇x, ̇y,t
end

mutable struct Point4Point4 <: IncrementalInference.FunctorPairwise where {T <: Distribution}
  z::T
  Point4Point4{T}() where {T <: Distribution} = new{T}()
  Point4Point4(z1::T) where {T <: Distribution} = new{T}(z1)
  Point4Point4(mean::Vector{Float64}, cov::Array{Float64,2}) where {T <: Distribution} = new{Distributions.MvNormal}(MvNormal(mean, cov))
end
getSample(p4p4::Point4Point4, N::Int=1) = (rand(s.z,N), )
function (p4p4::Point4Point4)(res::Array{Float64},
      idx::Int,
      meas::Tuple,
      xi::Array{Float64,5},
      xj::Array{Float64,5}  )
  Z = meas[1][1,idx]
  Xi, Xj = xj[:,idx],xj[:,idx]
  dt = Xj[5] - Xi[5] #implies Xj occured after Xi- potential issue?
  res = z - (Xj - (Xi+dt*[1 1 0 0]*Xi))
  nothing
end

Coverage is only at 22% right now ...

When using the Pose2Point2Bearing as landmark factor in Hexagonal example, two factors are added (x0l1 and x6l1). As the 2nd factor is added, a solver error is thrown.

There's a small example in the fixpose2point2brsample branch https://github.com/JuliaRobotics/RoME.jl/blob/hotfix/fixpose2point2brsample/examples/Hexagonal2D_BearingOnly_SLAM.jl

During the resolution of the non-linear system, the evaluation of the following equation(s) resulted in a non-finite number: [1]
in anonymous at base/<missing>
in macro expansion at Hexagonal2D_BearingOnly_SLAM.jl:45 
in addFactor! at IncrementalInference/src/FactorGraph01.jl:634
in #addFactor!#53 at IncrementalInference/src/FactorGraph01.jl:638
in  at base/<missing>
in #addFactor!#52 at IncrementalInference/src/FactorGraph01.jl:609
in  at base/<missing>
in #doautoinit!#45 at IncrementalInference/src/FactorGraph01.jl:497
in  at base/<missing>
in #predictbelief#136 at IncrementalInference/src/SolveTree01.jl:338
in  at base/<missing>
in #predictbelief#135 at IncrementalInference/src/SolveTree01.jl:313
in  at base/<missing>
in #proposalbeliefs!#134 at IncrementalInference/src/SolveTree01.jl:282
in findRelatedFromPotential at IncrementalInference/src/ApproxConv.jl:381
in  at base/<missing>
in #evalFactor2#130 at IncrementalInference/src/ApproxConv.jl:333
in  at base/<missing>
in #evalPotentialSpecific#127 at IncrementalInference/src/ApproxConv.jl:225
in computeAcrossHypothesis! at IncrementalInference/src/ApproxConv.jl:125
in approxConvOnElements! at IncrementalInference/src/ApproxConv.jl:68
in approxConvOnElements! at IncrementalInference/src/ApproxConv.jl:46
in #numericRootGenericRandomizedFnc!#114 at IncrementalInference/src/SolverUtilities.jl:94
in #nlsolve#39 at NLsolve/src/nlsolve/nlsolve.jl:59
in  at base/<missing>
in #nlsolve#38 at NLsolve/src/nlsolve/nlsolve.jl:26
in trust_region_ at NLsolve/src/solvers/trust_region.jl:120
in check_isfinite at NLsolve/src/nlsolve/utils.jl:35

When running the unit tests in the feature/multilangzmq branch, the tests in testpartialpose3.jl are failing due to NaN's in the points of x2.

https://github.com/JuliaRobotics/RoME.jl/blob/feature/multilangzmq/test/testpartialpose3.jl#L53

test PartialPriorRollPitchZ evaluations: Test Failed
  Expression: norm(X2pts[olddims, :] - pts[olddims, :]) < 1.0e-10
   Evaluated: NaN < 1.0e-10
Stacktrace:
 [1] macro expansion at /home/gearsad/.julia/v0.6/RoME/test/testpartialpose3.jl:54 [inlined]
 [2] macro expansion at ./test.jl:860 [inlined]
 [3] anonymous at ./<missing>:?

make difference between DynPoint2DynPoint2 clear from Point2Point2Velocity, as well as DynPose2 etc.

@JuliaRegistrator register branch=release/v0.5

tests failed with 53 < 15 ... restarted and will keep monitoring.

Maybe data can be stored in the softtype definitions, such as proposed in #60

test currently fails

@JuliaRegistrator register

@JuliaRegistrator register branch=release/v0.5

@JuliaRegistrator register branch=release/v0.5

I had a quick question about retrieving 2D samples from pose and landmark KDE's.

It looks like there's an API for get2DPoseSamples that looks like:

get2DPoseSamples(fg, from, to, api)

How many samples do I get from this?

Is this by default the number of samples associated with the marginal? Could we param this out so we can sample more or less? Maybe this functionality exists elsewhere?

I could also just explicitly loop over marginals and retrieve samples with:

kde = getVertKDE(fg, sym)
samples = sample(kde, N)[1]

but was wondering if this feature was exposed in the get2DPoseSamples or similar API somewhere. Thanks!

cc @keevindoherty

When running the unit tests in the feature/multilangzmq branch, the tests in testpartialpose3.jl are failing due to NaN's in the points of x2.

https://github.com/JuliaRobotics/RoME.jl/blob/feature/multilangzmq/test/testpartialpose3.jl#L53

test PartialPriorRollPitchZ evaluations: Test Failed
  Expression: norm(X2pts[olddims, :] - pts[olddims, :]) < 1.0e-10
   Evaluated: NaN < 1.0e-10
Stacktrace:
 [1] macro expansion at /home/gearsad/.julia/v0.6/RoME/test/testpartialpose3.jl:54 [inlined]
 [2] macro expansion at ./test.jl:860 [inlined]
 [3] anonymous at ./<missing>:?

WARNING: evaluateDualTree vector evaluation API is changing for single point evaluation across multiple dimensions rather than assuming multiple points on a univariate kde.

get2DLandmMax is one place where this shows up.

Travis Build 454.1

...
[TEST] Linear array function evaluations...
  0.094202 seconds (23.03 k allocations: 1.083 MiB)
zDim = size(fp!.measurement[1], 1) = 3
  0.816927 seconds (1.19 M allocations: 81.361 MiB, 5.08% gc time)
WARNING: still need to insert kld(..) test to ensure this is working
Test back projection from 
  0.000027 seconds (34 allocations: 2.484 KiB)
zDim = size(fp!.measurement[1], 1) = 3
  1.332480 seconds (4.98 M allocations: 370.005 MiB, 8.40% gc time)
WARNING: Breaks if not set to 100
Adding PriorPose3 to graph...
Adding LinearRangeBearingElevation to graph...
  0.000011 seconds (5 allocations: 208 bytes)
  0.296281 seconds (1.32 M allocations: 93.898 MiB, 10.52% gc time)
[drct]ERROR: LoadError: LoadError: numericRootGenericRandomizedFnc could not converge, i=6, fr.gwp.usrfnc!=RoME.LinearRangeBearingElevation
Stacktrace:
 [1] #numericRootGenericRandomizedFnc!#75(::Float64, ::Bool, ::Function, ::IncrementalInference.FastRootGenericWrapParam{RoME.LinearRangeBearingElevation}) at /home/travis/.julia/v0.6/IncrementalInference/src/SolverUtilities.jl:74
 [2] approxConvOnElements!(::IncrementalInference.FastRootGenericWrapParam{RoME.LinearRangeBearingElevation}, ::UnitRange{Int64}) at /home/travis/.julia/v0.6/IncrementalInference/src/ApproxConv.jl:19
 [3] computeAcrossHypothesis(::IncrementalInference.FastRootGenericWrapParam{RoME.LinearRangeBearingElevation}, ::Array{Any,1}, ::Array{Any,1}, ::UnitRange{Int64}, ::Int64) at /home/travis/.julia/v0.6/IncrementalInference/src/ApproxConv.jl:91
 [4] #evalPotentialSpecific#90(::Int64, ::Function, ::Array{Graphs.ExVertex,1}, ::IncrementalInference.GenericWrapParam{RoME.LinearRangeBearingElevation}, ::Int64) at /home/travis/.julia/v0.6/IncrementalInference/src/ApproxConv.jl:185
 [5] (::IncrementalInference.#kw##evalPotentialSpecific)(::Array{Any,1}, ::IncrementalInference.#evalPotentialSpecific, ::Array{Graphs.ExVertex,1}, ::IncrementalInference.GenericWrapParam{RoME.LinearRangeBearingElevation}, ::Int64) at ./<missing>:0
 [6] #evalFactor2#93(::Int64, ::Function, ::IncrementalInference.FactorGraph, ::Graphs.ExVertex, ::Int64) at /home/travis/.julia/v0.6/IncrementalInference/src/ApproxConv.jl:280
 [7] evalFactor2(::IncrementalInference.FactorGraph, ::Graphs.ExVertex, ::Int64) at /home/travis/.julia/v0.6/IncrementalInference/src/ApproxConv.jl:267
 [8] include_from_node1(::String) at ./loading.jl:576
 [9] include(::String) at ./sysimg.jl:14
 [10] include_from_node1(::String) at ./loading.jl:576
 [11] include(::String) at ./sysimg.jl:14
 [12] process_options(::Base.JLOptions) at ./client.jl:305
 [13] _start() at ./client.jl:371
while loading /home/travis/.julia/v0.6/RoME/test/testDidsonFunctions.jl, in expression starting on line 121
while loading /home/travis/.julia/v0.6/RoME/test/runtests.jl, in expression starting on line 22
================================[ ERROR: RoME ]=================================
failed process: Process(`/home/travis/julia/bin/julia -Cgeneric -J/home/travis/julia/lib/julia/sys.so --compile=yes --depwarn=yes --check-bounds=yes --code-coverage=user --color=no --compilecache=yes /home/travis/.julia/v0.6/RoME/test/runtests.jl`, ProcessExited(1)) [1]
================================================================================
ERROR: RoME had test errors
The command "julia --check-bounds=yes -e 'Pkg.clone(pwd()); Pkg.build("RoME"); Pkg.test("RoME"; coverage=true)'" exited with 1.
Done. Your build exited with 1.

Temporary fix till IIF v0.3.8+ is available

travis log

Based on testing with Caesar

LinearAlgebra
Distributed

@JuliaRegistrator register()