juliarobotics / caesar.jl Goto Github PK

https://github.com/dehann/Caesar.jl/blob/master/src/cloudgraphs/CloudGraphIntegration.jl

https://github.com/dehann/Caesar.jl/blob/b72f7486625da95d453d2040ea98f5ffb74a72c7/src/VisualizationUtils.jl#L25

can be fixed by:
gg = (x, a=0.0) -> evaluateDualTree(pl1, [x[1] x[2] x[3]]')[1]-a

When Caesar is unable to copy all nodes, it should fail and not update backendset = 1. It seems to do this even during a failure.

@tkoolen has done some nice work over at LCMCore.jl to make it very easy to make native Julia types to match LCM type definitions. It's not quite all the way to lcm-gen, but it's pretty easy to use and the resulting types should be extremely performant. I just ported all of the types in bot_core_lcmtypes and Drake last night.

If you're interested, I'm happy to open a PR to show what that would look like here. It should make the LCM interface much more efficient than using PyLCM.

multi-lang requests should, at the higher level, just have request and payload fields, e.g.:

{
  "request": "addFactor",
  "payload": {
    "factorType": "Prior",
    "measurements": {
      ...
    },
    "variables": [ "x0"]
  }
}

See also: graff_cpp/pull/14

Some julia 0.6 issues in example, also some plots does not work for probably the same reason.
https://github.com/dehann/Caesar.jl/blob/325dc445ce918d09de401b67f4691e51fa8457e5/examples/wheeled/victoriapark_stepbystep.jl#L15
change to

evaluateDualTree(p, point[:,:])[1] 

Also function not found:
https://github.com/dehann/Caesar.jl/blob/325dc445ce918d09de401b67f4691e51fa8457e5/examples/wheeled/victoriapark_stepbystep.jl#L152

Bring example up to the new Caesar.jl API

To help general readability, maybe there should be a common file which lists convenience functions on top of the more hardy API. For example:

• xref #119
• xref #71
• xref #72

identitypose6fg! see here

cc @GearsAD

@nicrip found that his insertion script would not always have 100% input success. Will link file in comment below.

When running drawdbdirector()

ERROR: dont know how to deal with data type=Array{Any,1}
 in #prepcolordepthcloud!#151(::Array{Any,1}, ::Int64, ::Float64, ::Function, ::Int64, ::Array{Any,1}) at /home/pvt/.julia/v0.5/Caesar/src/cloudgraphs/DirectorVisService.jl:111
 in (::Caesar.#kw##prepcolordepthcloud!)(::Array{Any,1}, ::Caesar.#prepcolordepthcloud!, ::Int64, ::Array{Any,1}) at ./<missing>:0
 in #fetchdrawdepthcloudbycvid!#157(::Dict{String,String}, ::CoordinateTransformations.AffineMap{Rotations.Quat{Float64},StaticArrays.SVector{3,Float64}}, ::Function, ::DrakeVisualizer.Visualizer, ::CloudGraphs.CloudGraph, ::Int64, ::Symbol, ::Dict{Any,Any}, ::Dict{String,Any}, ::Symbol) at /home/pvt/.julia/v0.5/Caesar/src/cloudgraphs/DirectorVisService.jl:265
 in (::Caesar.#kw##fetchdrawdepthcloudbycvid!)(::Array{Any,1}, ::Caesar.#fetchdrawdepthcloudbycvid!, ::DrakeVisualizer.Visualizer, ::CloudGraphs.CloudGraph, ::Int64, ::Symbol, ::Dict{Any,Any}, ::Dict{String,Any}, ::Symbol) at ./<missing>:0
 in #fetchdrawposebycvid!#159(::String, ::Dict{String,String}, ::Function, ::DrakeVisualizer.Visualizer, ::CloudGraphs.CloudGraph, ::Int64, ::Dict{Any,Any}, ::Dict{String,Any}) at /home/pvt/.julia/v0.5/Caesar/src/cloudgraphs/DirectorVisService.jl:323
 in (::Caesar.#kw##fetchdrawposebycvid!)(::Array{Any,1}, ::Caesar.#fetchdrawposebycvid!, ::DrakeVisualizer.Visualizer, ::CloudGraphs.CloudGraph, ::Int64, ::Dict{Any,Any}, ::Dict{String,Any}) at ./<missing>:0
 in macro expansion at /home/pvt/.julia/v0.5/Caesar/src/cloudgraphs/DirectorVisService.jl:375 [inlined]
 in macro expansion at /home/pvt/.julia/v0.5/ProgressMeter/src/ProgressMeter.jl:474 [inlined]
 in drawdbsession(::DrakeVisualizer.Visualizer, ::CloudGraphs.CloudGraph, ::Dict{AbstractString,Union{AbstractString,Array{String,1}}}, ::Dict{String,Any}, ::Dict{Any,Any}) at /home/pvt/.julia/v0.5/Caesar/src/cloudgraphs/DirectorVisService.jl:374
 in #drawdbdirector#160(::Dict{AbstractString,Union{AbstractString,Array{String,1}}}, ::Function) at /home/pvt/.julia/v0.5/Caesar/src/cloudgraphs/DirectorVisService.jl:436
 in (::Caesar.#kw##drawdbdirector)(::Array{Any,1}, ::Caesar.#drawdbdirector) at ./<missing>:0

Key at fault: bDE.mongoKey = "5903a2d56689bc076e31e2d7"

... took the liberty of looking at some of your code on GitHub. Although it is clean and well-organized, I couldn't really follow it, as the names are generally short and, to me, inscrutable. Many of the function names are descriptive, but most of the variable names are one or two letters. It would help me if the names were evocative.

Current examples show particle distribution, but e.g. RoME.jl readme shows contour plot of distribution

Please add your comments or support for this requirement here.

• executeQuery should become executeQuery!,
• ensure Neo4j.commit always occurs in executeQuery!

cc @GearsAD

For beginner users - when should I create poses?

Majority of sensors sample quickly - e.g. 30 samples/sec - should I save a pose every time I get an update? The majority of that data is redundant, but is there a good rule-of-thumb for selecting what to push as a new pose?

I'm not seeing any MAP_est properties in the graph once the solver has run? This seems to have cropped up in the new code.

Link no longer exists, must fix

Add poses/factors for: as example an AHRS that gives an orientation relative to some world yaw offset that is a parameter in the node that changes slowly over time.

Initialization seems to be failing intermittently while running real-time. Getting this error when running the solver:

===================CONVERT===================
done populating new variables
WARNING: using hack counter for FACTOR uid starting at 100052

=============ENSURE INITIALIZED==============

================MULTI-SESSION================

====================SOLVE====================
Get local copy of graph
fullLocalGraphCopy: 50 nodes in session RoverRover if reqbackendset=true and reqready=true...
Copy all nodes...100%|██████████████████████████████████| Time: 0:00:03
-------------Ensure Initialization-----------
x22 is not initialized, and will do so now...
Looking for cloud out neighbors
cgid = fgl.cgIDs[exVertId] = 600
neinodes = ls(fgl, vsym) = Symbol[:x22x23f1, :x21x22f1]
Looking for cloud out neighbors
cgid = fgl.cgIDs[exVertId] = 627
xfneivarnodes = lsf(fgl, xifct) = Symbol[:x23, :x22]
Looking for cloud out neighbors
cgid = fgl.cgIDs[exVertId] = 1282
xfneivarnodes = lsf(fgl, xifct) = Symbol[:x22, :x21]
 --- Exiting! Deregistering solver with ID '9e994028-749b-4821-9127-c7061a330cb4'!
ERROR: LoadError: BoundsError: attempt to access 0×0 Array{Float64,2} at index [Base.Slice(Base.OneTo(0)), 1]
while loading /home/gearsad/.julia/v0.6/CaesarServer/scripts/runme.jl, in expression starting on line 8

The node in question contains the following data

bigData: {"isRetrieved":false,"isAvailable":false,"isExistingOnServer":true,"lastSavedTimestamp":"2018-04-28T02:56:13.569","version":"2","dataElements":[]} data: 16,0,32,0,48,0,64,0,80,1,80,2,80,3,88,3,96,0,104,0,122,43,73,110,99,114,101,109,101,110,116,97,108,73,110,102,101,114,101,110,99,101,46,67,111,110,116,105,110,117,111,117,115,77,117,108,116,105,118,97,114,105,97,116,101,128,1,1 exVertexId: 47 label: x23 neighborVertexIDs: 627 packedType: IncrementalInference.PackedVariableNodeData ready: 1

see JuliaRobotics/RoME.jl#84

Here is a factor graph where
ls(fg, :l11)
1-element Array{Symbol,1}:
:x109l11f1

But

ls(fg, :x3)
5-element Array{Symbol,1}:
:x2x3f1
:x3l11f1
:x3l12f1
:x3l13f1
:x3x4f1

How is the factor graph being built?

You may have mentioned this, but wanted to confirm. I'm seeing deserialization issues when solving, if I upgrade everything to latest and reinsert the graphs, would that resolve the issue?

ERROR: LoadError: MethodError: Cannot convert an object of type Type{RoME.PackedPriorPose2} to an object of type IncrementalInference.FunctorInferenceType
This may have arisen from a call to the constructor IncrementalInference.FunctorInferenceType(...),
since type constructors fall back to convert methods.
Stacktrace:
[1] decodePackedType(::IncrementalInference.GenericFunctionNodeData{RoME.PackedPriorPose2,String}, ::String) at /home/gearsad/.julia/v0.6/IncrementalInference/src/DispatchPackedConversions.jl:293
[2] unpackNeoNodeData2UsrType(::CloudGraphs.CloudGraph, ::Neo4j.Node) at /home/gearsad/.julia/v0.6/CloudGraphs/src/CloudGraphs.jl:135
[3] neoNode2CloudVertex(::CloudGraphs.CloudGraph, ::Neo4j.Node) at /home/gearsad/.julia/v0.6/CloudGraphs/src/CloudGraphs.jl:144
[4] get_vertex(::CloudGraphs.CloudGraph, ::Int64, ::Bool) at /home/gearsad/.julia/v0.6/CloudGraphs/src/CloudGraphs.jl:217
[5] macro expansion at /home/gearsad/.julia/v0.6/Caesar/src/cloudgraphs/CloudGraphIntegration.jl:581 [inlined]
[6] macro expansion at /home/gearsad/.julia/v0.6/ProgressMeter/src/ProgressMeter.jl:483 [inlined]
[7] copyAllNodes!(::IncrementalInference.FactorGraph, ::Dict{Int64,CloudGraphs.CloudVertex}, ::Array{Tuple{Int64,Int64,Symbol},1}) at /home/gearsad/.julia/v0.6/Caesar/src/cloudgraphs/CloudGraphIntegration.jl:580
[8] copyGraphNodesEdges!(::IncrementalInference.FactorGraph, ::Array{Tuple{Int64,Int64,Symbol},1}) at /home/gearsad/.julia/v0.6/Caesar/src/cloudgraphs/CloudGraphIntegration.jl:607
[9] #fullLocalGraphCopy!#32(::Bool, ::Bool, ::Function, ::IncrementalInference.FactorGraph) at /home/gearsad/.julia/v0.6/Caesar/src/cloudgraphs/CloudGraphIntegration.jl:652
[10] runSlamInDbOnSession(::Caesar.CaesarConfig, ::CloudGraphs.CloudGraph, ::String, ::String, ::String, ::Int64, ::Bool, ::Caesar.SolverStatus) at /home/gearsad/.julia/v0.6/Caesar/src/cloudgraphs/slamindb.jl:76
[11] runCaesarOnSessionWithLogging(::CaesarServer.SystemConfig, ::CloudGraphs.CloudGraph, ::String, ::String, ::String, ::Int64, ::Bool, ::Caesar.SolverStatus, ::Bool) at /home/gearsad/.julia/v0.6/CaesarServer/src/services/CaesarLogService.jl:65
[12] startCaesarServer() at /home/gearsad/.julia/v0.6/CaesarServer/src/CaesarServer.jl:126
[13] include_from_node1(::String) at ./loading.jl:576
[14] include(::String) at ./sysimg.jl:14
[15] process_options(::Base.JLOptions) at ./client.jl:305
[16] _start() at ./client.jl:371

For beginner users: When creating a robot-side process to push data into Caesar.jl, should I always assume that every sessions starts at the origin and that subsequent poses are relative to that root vertex?

This affects whether I should create a pose the moment I start. If so, what happens when the factor graph is refined? Bigger question, but this is a start to a longer discussion.

Is there a simple example making use of the multi-hypothesis API?

This question came up: is it possible to only partially constrain a variable by using mix of factors. For example a landmark with orientation is stored as a Pose2, but now we only have bearing and/or range data to that landmark. How to programmatically resolve this?

cc @keevindoherty

using Caesar
fg = ...

X1 = getVertKDE(fg, :x1) # this is a ::KernelDensityEstimate.BallTreeDensity

KernelDensityEstimate.evaluateDualTree(X1,[-2.0;-1;0;1;2])
# 5-element Array{Float64,1}:
# ...

Should add this to Caesar.jl's documentation and probably improve API through IIF/RoME/Caesar to simplify usage.

For example, if I have all Gaussian factors, is there an easy way within Caesar to see what the uni-modal solution would be (e.g. as if I were using iSAM2)

Two options for getting next pose name with addOdoFG:

x$(i+1)

or

:VARIABLE:POSE?

I'd like to keep the ability open for users to call their variables different names, for example :x1 vs :p1 vs :f1. Think of a robot having pelvis, head, left foot, right foot: A new time instant might create all of :p1, :h1, :lf1, :rf1, etc.

Remember addOdoFG should not drive design decisions, it is a convenience function.

cc @GearsAD

@GearsAD FYI, please note that config should not use Bool. I use default:

"multiSession": [""],

This is actually an LCMCore dependency that isn't met, but appears when Caesar is used and it in turn uses LCMCore. Just writing as a placeholder.

There is a significant body of research on terrain-aided navigation, which could maybe be described as "scalar field"-aided navigation, so that it generalizes to applications where instead of altitude, we use the concentration of a certain chemical compound or a temperature field. Beyond this, we'd go into vector fields - magnetic, gravity, flow, ...

Can we promote Caesar.usecloudgraphsdatalayer to RoME? Simplifies dependencies that way.

Hello Dehann,
Please add some more documentation on functions. Maybe with Documenter.jl
Thanks

After running inferOverTree! , this is how you would draw poses and landmarks:

using RoMEPlotting

# produce a figure in the julia workspace
pl = drawPoses(fg)

# using Gadfly
Gadfly.draw(PDF("/tmp/test.pdf", 20cm,10cm),pl)  # or PNG(...)
# or
drawPosesLandms(fg)

cc @keevindoherty

Fix this example for general use.

see branch feature/multilangzmq

Getting the full exception back as a reply isn't very helpful; a simple message telling the user to look at the endpoint output/debug log should be enough.

catch ex
   io = IOBuffer()
  showerror(io, ex, catch_backtrace())
  err = String(take!(io))
  warn("[ZMQ Server] Exception: $err")
  resp["status"] = "ERROR"
  resp["error"] = "Endpoint exception; please check the endpoint output/log"
end

see:

• https://juliacomputing.com/blog/2016/02/09/static-julia.html
• https://github.com/JuliaLang/PackageCompiler.jl
• https://discourse.julialang.org/t/compiler-work-priorities/17623

I'm able to save empty factor graphs, but I can't save a graph with a few poses in it.

This seems like an upstream issue with IncrementalInference.jl; and could be further upstream with HDF5. Let me know if you have any thoughts about how we should address this. Is there a dependency on a specific version of JLD/HDF5?

Stack trace below:

HDF5-DIAG: Error detected in HDF5 (1.10.0-patch1) thread 140193041247360:
  #000: ../../../src/H5Tfields.c line 62 in H5Tget_nmembers(): not a datatype
    major: Invalid arguments to routine
    minor: Inappropriate type
HDF5-DIAG: Error detected in HDF5 (1.10.0-patch1) thread 140193041247360:
  #000: ../../../src/H5Tfields.c line 62 in H5Tget_nmembers(): not a datatype
    major: Invalid arguments to routine
    minor: Inappropriate type
ERROR: Error getting the number of members
Stacktrace:
 [1] h5t_get_nmembers at /home/kevin/.julia/v0.6/HDF5/src/HDF5.jl:2264 [inlined]
 [2] _gen_h5convert!(::Any) at /home/kevin/.julia/v0.6/JLD/src/jld_types.jl:631
 [3] h5convert!(...) at /home/kevin/.julia/v0.6/JLD/src/jld_types.jl:657
 [4] #write_compound#21(::Array{Any,1}, ::Function, ::JLD.JldFile, ::String, ::IncrementalInference.FactorGraph, ::JLD.JldWriteSession) at /home/kevin/.julia/v0.6/JLD/src/JLD.jl:698
 [5] #write#14(::Array{Any,1}, ::Function, ::JLD.JldFile, ::String, ::IncrementalInference.FactorGraph, ::JLD.JldWriteSession) at /home/kevin/.julia/v0.6/JLD/src/JLD.jl:512
 [6] write(::JLD.JldFile, ::String, ::IncrementalInference.FactorGraph, ::JLD.JldWriteSession) at /home/kevin/.julia/v0.6/JLD/src/JLD.jl:512
 [7] macro expansion at /home/kevin/.julia/v0.6/JLD/src/JLD.jl:1159 [inlined]
 [8] #savejld#45(::String, ::Void, ::Function, ::IncrementalInference.FactorGraph) at /home/kevin/.julia/v0.6/IncrementalInference/src/FGOSUtils.jl:19
 [9] (::IncrementalInference.#kw##savejld)(::Array{Any,1}, ::IncrementalInference.#savejld, ::IncrementalInference.FactorGraph) at ./<missing>:0

Multiple users have the same session name.