Merge pull request #602 from JuliaRobotics/master

v0.6.1-rc1

Project.toml

@@ -2,7 +2,7 @@ name = "Caesar"
 uuid = "62eebf14-49bc-5f46-9df9-f7b7ef379406"
 keywords = ["SLAM", "state-estimation", "mm-iSAM", "inference", "robotics", "ROS"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.6.0"
+version = "0.6.1"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -33,13 +33,12 @@ Requires = "ae029012-a4dd-5104-9daa-d747884805df"
 RoME = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
-Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
+TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 Unmarshal = "cbff2730-442d-58d7-89d1-8e530c41eb02"
 YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6"
-ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 
 [compat]
 ApproxManifoldProducts = "^0.1"
@@ -64,6 +63,7 @@ Reexport = "0.2"
 Requires = "0.5, 0.6, 0.7, 0.8, 0.9, 0.10, 1"
 RoME = "0.7.1, 0.8, 0.9"
 Rotations = "0.13, 1.0"
+TensorCast = "0.2, 0.3"
 TimeZones = "1.3.1"
 TransformUtils = "^0.2.2"
 Unmarshal = "0.2.1, 0.3"
@@ -72,10 +72,13 @@ ZMQ = "1"
 julia = "1.4"
 
 [extras]
+AprilTags = "f0fec3d5-a81e-5a6a-8c28-d2b34f3659de"
+Images = "916415d5-f1e6-5110-898d-aaa5f9f070e0"
 PyCall = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0"
 RobotOS = "22415677-39a4-5241-a37a-00beabbbdae8"
 RoMEPlotting = "238d586b-a4bf-555c-9891-eda6fc5e55a2"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
+ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 
 [targets]
-test = ["Test", "PyCall", "RobotOS"]
+test = ["Test", "PyCall", "RobotOS", "ZMQ", "AprilTags", "Images"]

docs/src/func_ref.md

@@ -114,13 +114,9 @@ getParent
 getTreeAllFrontalSyms
 getTreeCliqSolveOrderUp
 getTreeCliqsSolverHistories
-getVal
 getVariableDim
 getVariableInferredDim
-getUpMsgs
-getDwnMsgs
-hasCliq
-hasOrphans
+hasClique
 initfg
 initInferTreeUp!
 isCliqMarginalizedFromVars

examples/dev/apriltagserver/tag_exporter.jl

@@ -21,6 +21,7 @@ function commands()
   parse_args(ARGS, s)
 end
 
+# use AprilTags.drawTags instead
 function showImage(image, tags)
     # Convert image to RGB
     imageCol = RGB.(image)

examples/wheeled/racecar/RedrawTagDetails.jl

@@ -0,0 +1,76 @@
+
+
+using Flux, RoME, RoMEPlotting
+using ImageMagick, Images, ImageView, ImageDraw
+using Colors
+using JSON2
+
+using FreeTypeAbstraction, AprilTags
+
+
+function imdrawline!(img, col; color=RGB(1.0,0.0,0.0) )
+  for i in 1:size(img, 1)
+    img[i, col] = color
+  end
+end
+
+
+
+cd("/media/dehann/logs3/caesar/2020-08-19T11:45:38.074")
+
+fg = loadDFG("fg_final_resolve.tar.gz")
+
+# wire up the blob store
+getSolverParams(fg).logpath = pwd()
+storeDir = joinLogPath(fg,"data")
+mkpath(storeDir)
+datastore = FolderStore{Vector{UInt8}}(:default_folder_store, storeDir) 
+addBlobStore!(fg, datastore)
+
+
+# Retrieve camera calibration
+listDataEntries(fg, :x0)
+rD = getData(fg, :x0, :camCalib)
+camCalib = JSON2.read(IOBuffer(rD[2]))
+K = reshape(camCalib[:vecK], camCalib[:size]...) .|> Float64
+
+
+# get image from x1
+imgE ,imgD = getData(fg, :x1, :KEYFRAME_IMG)
+img = ImageMagick.readblob(imgD);
+# check that we have the image
+imshow(img)
+
+
+# detect and draw tags
+detector = AprilTagDetector()
+tags = detector(img)
+
+
+
+img_ = drawTags(img,K, tags)
+imshow(img_)
+
+
+ls(fg, :x1)
+
+x1l2f1 = getFactorType(fg, :x1l2f1)
+
+x1l2f1.z.μ
+
+
+function calcBearingRange(dfg::AbstractDFG, from::Symbol, to::Symbol)
+  getSofttype(dfg, from)
+end
+
+
+
+measimgcol = round(Int,camc-camf*tan(getfnctype(ve).bearing.μ)) # measurement center
+imdrawline!(img, measimgcol)
+
+
+# deptdist = Base.mean(depthcloud[260:270,measimgcol,3])
+# push!(DISTS[fct], deptdist)
+
+
+

examples/wheeled/racecar/dev/testAT.jl

@@ -10,6 +10,7 @@ using AprilTags
 # @pyimport numpy as np
 # @pyimport cv2
 
+# deprecated, use AprilTags.drawTags instead
 function showImage(image, tags, K)
     # Convert image to RGB
     imageCol = RGB.(image)

examples/wheeled/racecar/ros/CarSlamUtilsCommon.jl

@@ -95,29 +95,6 @@ end
 
 
 
-## Visualization functions
-
-function showImage(image, tags, K)
-    # Convert image to RGB
-    imageCol = RGB.(image)
-    # draw the tag number for each tag
-    foreach(x->drawTagID!(imageCol, x),tags)
-    #draw color box on tag corners
-    foreach(tag->drawTagBox!(imageCol, tag, width = 2, drawReticle = false), tags)
-    foreach(tag->drawTagAxes!(imageCol,tag, K), tags)
-    imageCol
-end
-
-
-
-function fetchDataElement(dfg::AbstractDFG, varsym::Symbol, lbl::Symbol)
-    gde,rawData = getData(dfg, varsym, lbl)
-    # entry = getBigDataEntry(var, lbl)
-    # rawData = getBigData(datastore, entry)
-    # # raw data is json-encoded; this decoding should happen inside getBigData?
-    return JSON2.read(IOBuffer(rawData))
-end
-
 
 function jsonResultsSLAM2D(dfg::AbstractDFG)
   allDicts = []
@@ -172,19 +149,6 @@ function jsonResultsSLAM2D(dfg::AbstractDFG)
 end
 
 
-# load image (png, jpg, jpeg) from supported a data blob store 
-function fetchDataImage(dfg::AbstractDFG,
-                        varLbl::Symbol,
-                        dataLbl::Symbol,
-                        getDataLambda::Function = (g,vl,dl) -> getData(g,vl,dl),
-                        checkMimeType::Bool=true )
-  #
-  imgEntry, imgBytes = getDataLambda(dfg, varLbl, dataLbl)
-  checkMimeType && (@assert imgEntry.mimeType in ["image/png"; "image/jpg"; "image/jpeg"] "Unknown image format DataBlobEntry.mimeType=$(imgEntry.mimeType)")
-  ImageMagick.readblob(imgBytes)
-end
-fetchDataImage(dfg::AbstractDFG,datastore::AbstractBlobStore,varLbl::Symbol,dataLbl::Symbol,checkMimeType::Bool=true) = fetchDataImage(dfg, varLbl, dataLbl, (g,vl,dl) -> getData(g,datastore,vl,dl) , checkMimeType)
-
 
 # reproject a bearing range onto (assumed level) image.
 

examples/wheeled/racecar/ros/CarSlamUtilsMono.jl

@@ -272,7 +272,7 @@ function drawLatestImage(fec::FrontEndContainer; syncList=[:leftFwdCam;])
   tagsL = detector(fec.synchronizer.leftFwdCam[idxL][3] |> collect)
 
   # @show poses = (T->homographytopose(T.H, fx, fy, cx, cy, taglength = 160.)).(tagsL)
-  imgLt = showImage(fec.synchronizer.leftFwdCam[idxL][3], tagsL, K)
+  imgLt = AprilTags.drawTags(fec.synchronizer.leftFwdCam[idxL][3], K, tagsL) # showImage
 
   # imgL = syncImgs[:left][idxL][2] .|> Gray
   # imgR = syncImgs[:right][idxR][2] .|> Gray

examples/wheeled/racecar/ros/CarSlamUtilsStereo.jl

@@ -65,8 +65,8 @@ function drawLatestImagePair(syncImgs)
   tagsR = detector(syncImgs[:right][idxR][2])
 
   # @show poses = (T->homographytopose(T.H, fx, fy, cx, cy, taglength = 160.)).(tagsL)
-  imgLt = showImage(syncImgs[:left][idxL][2], tagsL, K)
-  imgRt = showImage(syncImgs[:right][idxR][2], tagsR, K)
+  imgLt = AprilTags.drawTags(syncImgs[:left][idxL][2], K, tagsL)  # showImage
+  imgRt = AprilTags.drawTags(syncImgs[:right][idxR][2], K, tagsR)  # showImage
 
   # imgL = syncImgs[:left][idxL][2] .|> Gray
   # imgR = syncImgs[:right][idxR][2] .|> Gray

src/Caesar.jl

@@ -21,7 +21,6 @@ using
   Statistics,
   LinearAlgebra,
   IncrementalInference,
-  # Graphs,
   TransformUtils,
   CoordinateTransformations,
   Rotations,
@@ -30,16 +29,14 @@ using
   FileIO,
   DataStructures,
   ProgressMeter,
-  ImageMagick,
+  ImageMagick, # figure out who else is using this and move to requires
   ImageCore,
   DocStringExtensions,
-  # CloudGraphs, # TODO: will be movedd to DFG
-  # Neo4j, # TODO: will be movedd to DFG
-  # Mongoc, # TODO: will be movedd to DFG
   Unmarshal,
   YAML,
   FFTW,
-  TimeZones
+  TimeZones,
+  TensorCast
 
 export
   GenericInSituSystem,  # insitu components
@@ -56,11 +53,6 @@ export
   tcpStringSLAMServer,
   tcpStringBRTrackingServer,
 
-  # save and load data
-  saveSlam, # TODO deprecate
-  loadSlam, # TODO deprecate
-  haselement,
-
   # user functions
   identitypose6fg,
   projectrbe,
@@ -84,7 +76,6 @@ export
   reset!,
   prepMF,
   loadConfigFile,
-  prepareRangeModel,
   prepareSAS2DFactor,
   wrapRad,
   phaseShiftSingle!,
@@ -93,7 +84,7 @@ export
 
 
 
-NothingUnion{T} = Union{Nothing, T}
+const NothingUnion{T} = Union{Nothing, T}
 
 include("BearingRangeTrackingServer.jl")
 
@@ -105,19 +96,8 @@ include("UserFunctions.jl")
 include("config/CaesarConfig.jl")
 
 
-# using CloudGraphs
-# include("attic/cloudgraphs/SolverStatus.jl")
-# include("attic/cloudgraphs/IterationStatistics.jl")
-# include("attic/cloudgraphs/CloudGraphIntegration.jl") # Work in progress code
-# include("attic/cloudgraphs/ConvertGeneralSlaminDB.jl")
-# include("attic/cloudgraphs/slamindb.jl")
-# include("attic/cloudgraphs/MultisessionUtils.jl")
-# include("attic/cloudgraphs/FoveationUtils.jl")
-
 include("Deprecated.jl")
 
-# ZMQ server and endpoints
-include("zmq/ZmqCaesar.jl")
 
 # Multisession operation
 # include("attic/multisession/Multisession.jl")
@@ -131,14 +111,16 @@ include("beamforming/SASUtils.jl")
 
 # conditional loading for ROS
 function __init__()
+  # ZMQ server and endpoints
+  @require ZMQ="c2297ded-f4af-51ae-bb23-16f91089e4e1" include("zmq/ZmqCaesar.jl")
   @require PyCall="438e738f-606a-5dbb-bf0a-cddfbfd45ab0" begin
     @info "Loading Caesar PyCall specific utilities (using PyCall)."
     @eval using .PyCall
-    @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" begin
-      @info "Loading Caesar ROS specific utilities (using RobotOS)."
-      include("ros/Utils/RosbagSubscriber.jl")
-    end
+    @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("ros/Utils/RosbagSubscriber.jl")
   end
+  @require AprilTags="f0fec3d5-a81e-5a6a-8c28-d2b34f3659de" include("images/apriltags.jl")
+  @require ImageMagick="6218d12a-5da1-5696-b52f-db25d2ecc6d1" include("images/imagedata.jl")
+  @require Images="916415d5-f1e6-5110-898d-aaa5f9f070e0" include("images/images.jl")
 end
 
 end

src/DataUtils.jl

@@ -1,31 +1,6 @@
 
 export getGitCommitSHA
 
-function saveSlam(slamwrapper::SLAMWrapper; filename::AbstractString="tempSlam.jld")
-  saveslam = deepcopy(slamwrapper);
-  @warn "Saving factor graph and landmark index -- unresolved issue with saving current tree, but can be reccomputed from factor graph with IncrementalInference.wipeBuildNewTree!(...)."
-  saveslam.tree = Union{}
-  jldopen(filename,"w") do file
-    write(file, "slam", saveslam)
-  end
-  nothing
-end
-
-function loadSlam(; filename::AbstractString="tempSlam.jld")
-  d = jldopen(filename,"r") do file
-    read(file, "slam", slam)
-  end
-  return d
-end
-
-function haselement(arr::Vector{T}, val::T) where {T}
-  for a in arr
-    if a == val
-      return true
-    end
-  end
-  return false
-end
 
 """
     $SIGNATURES
@@ -41,4 +16,7 @@ function getGitCommitSHA(package_name::AbstractString)
   return commit
 end
 
+
+
+
 #

src/images/apriltags.jl

@@ -0,0 +1,8 @@
+
+@info "Loading Caesar tools related to AprilTags.jl."
+
+# using Images
+
+
+
+#