From b8da7e6392754d21cbaeee012bac531c3c76b065 Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 20 Jul 2018 23:31:55 -0400 Subject: [PATCH 01/47] starting work on racecar apriltag example --- examples/wheeled/racecar/cam_cal.yml | 4 ++++ .../racecar/quickAprilTagSLAM_local.jl | 24 +++++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 examples/wheeled/racecar/cam_cal.yml create mode 100644 examples/wheeled/racecar/quickAprilTagSLAM_local.jl diff --git a/examples/wheeled/racecar/cam_cal.yml b/examples/wheeled/racecar/cam_cal.yml new file mode 100644 index 000000000..dc6da40b1 --- /dev/null +++ b/examples/wheeled/racecar/cam_cal.yml @@ -0,0 +1,4 @@ +date: 2018-07-20 +extrinsics: + # bV = bRc*cV bodyXYZ=FWD-PRT-OVH, cameraXYZ=STB-DWN-FWD + bRc: [0 0 1; -1 0 0; 0 -1 0] diff --git a/examples/wheeled/racecar/quickAprilTagSLAM_local.jl b/examples/wheeled/racecar/quickAprilTagSLAM_local.jl new file mode 100644 index 000000000..d881c8d2d --- /dev/null +++ b/examples/wheeled/racecar/quickAprilTagSLAM_local.jl @@ -0,0 +1,24 @@ +# Local compute version + +using Caesar +using YAML + +function loadConfig() + cfg = Dict{Symbol,Any}() + data = YAML.load(open(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cam_cal.yml"))) + bRc = eval(parse("["*data["extrinsics"]["bRc"][1]*"]")) + cfg[:bRc] = bRc + cfg +end + + +cfg = loadConfig() + +datafolder = ENV["HOME"]*"/data/racecar/smallloop/" +@load datafolder*"tag_det_per_pose.jld" tag_bag + + + + + +# From e61eda819d91740d4630b9331608b15b5455875a Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 24 Jul 2018 12:45:46 -0400 Subject: [PATCH 02/47] wip racecar frontend and calibration --- examples/wheeled/racecar/cam_cal.yml | 9 + examples/wheeled/racecar/ensureCamCal.jl | 175 ++++++++++++++++++ .../racecar/quickAprilTagSLAM_local.jl | 152 ++++++++++++++- examples/wheeled/racecar/racecarUtils.jl | 113 +++++++++++ src/Caesar.jl | 3 +- 5 files changed, 449 insertions(+), 3 deletions(-) create mode 100644 examples/wheeled/racecar/ensureCamCal.jl create mode 100644 examples/wheeled/racecar/racecarUtils.jl diff --git a/examples/wheeled/racecar/cam_cal.yml b/examples/wheeled/racecar/cam_cal.yml index dc6da40b1..374ebb0c8 100644 --- a/examples/wheeled/racecar/cam_cal.yml +++ b/examples/wheeled/racecar/cam_cal.yml @@ -2,3 +2,12 @@ date: 2018-07-20 extrinsics: # bV = bRc*cV bodyXYZ=FWD-PRT-OVH, cameraXYZ=STB-DWN-FWD bRc: [0 0 1; -1 0 0; 0 -1 0] + +# taken from ROS topic and ROS ZED driver process. +intrinsics: + width: 672 + height: 376 + camera_matrix: [340.97913 0.00000 330.41730; 0.00000 340.97913 196.32587; 0.00000 0.00000 1.00000] + distortion: [0.00000 0.00000 0.00000 0.00000 0.00000] + rectification: [1.00000 0.00000 0.00000; 0.00000 1.00000 0.00000; 0.00000 0.00000 1.00000] + projection: [340.97913 0.00000 330.41730 -0.04092; 0.00000 340.97913 196.32587 0.00000; 0.00000 0.00000 1.00000 0.00000] diff --git a/examples/wheeled/racecar/ensureCamCal.jl b/examples/wheeled/racecar/ensureCamCal.jl new file mode 100644 index 000000000..a1e16f1d8 --- /dev/null +++ b/examples/wheeled/racecar/ensureCamCal.jl @@ -0,0 +1,175 @@ +# trying to see range bearing AprilTag detections + +using Images, ImageDraw, ImageView, FileIO +using YAML, JLD, HDF5 +using Colors + +using AprilTags + + + +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) + + +cfg = loadConfig() +# lazy bad ... sorry +cw, ch = 330.4173, 196.32587 +focal = 340.97913 +tagsize = 0.172 + + +datafolder = ENV["HOME"]*"/data/racecar/smallloop/" +@load datafolder*"tag_det_per_pose.jld" tag_bag +# tag_bag[0] + +camcount = readdlm(datafolder*"images/cam-count.csv",',') +camlookup = Dict{Int, String}() +for i in 1:size(camcount,1) + camlookup[camcount[i,1]] = strip(camcount[i,2]) +end + + + +IMGS = [] + + +# pose :x0 + +img = load(datafolder*"images/left0005.jpg") + +push!(IMGS, deepcopy(img)) + +drawTagLine!(IMGS[1], tag_bag[0][0]) +drawTagLine!(IMGS[1], tag_bag[0][1]) +drawTagLine!(IMGS[1], tag_bag[0][16]) +drawTagLine!(IMGS[1], tag_bag[0][584]) + + + +IMGS[1] + + + +# pose :x1 +psid = 1 + +pose_tags = tag_bag[psid] +img = load(datafolder*"images/$(camlookup[psid])") +push!(IMGS, deepcopy(img)) + +drawTagLine!(IMGS[2], pose_tags[0]) +drawTagLine!(IMGS[2], pose_tags[1]) +drawTagLine!(IMGS[2], pose_tags[16]) +drawTagLine!(IMGS[2], pose_tags[584]) + + +IMGS[2] + + + + + + + +# pose :x10 +psid = 10 + +pose_tags = tag_bag[psid] +img = load(datafolder*"images/$(camlookup[psid])") +push!(IMGS, deepcopy(img)) + +drawTagLine!(IMGS[end], pose_tags[0]) +drawTagLine!(IMGS[end], pose_tags[1]) +drawTagLine!(IMGS[end], pose_tags[16]) + + +IMGS[end] + + + + + + + + + +# pose :x25 +psid = 25 + +pose_tags = tag_bag[psid] +img = load(datafolder*"images/$(camlookup[psid])") +push!(IMGS, deepcopy(img)) + + + +drawTagLine!(IMGS[end], pose_tags[8]) +drawTagLine!(IMGS[end], pose_tags[9]) +drawTagLine!(IMGS[end], pose_tags[10]) +drawTagLine!(IMGS[end], pose_tags[16]) + + +IMGS[end] + + + + + + + + + +# pose :x25 +psid = 26 + +pose_tags = tag_bag[psid] +img = load(datafolder*"images/$(camlookup[psid])") +push!(IMGS, deepcopy(img)) + + +drawTagLine!(IMGS[end], pose_tags[9]) +drawTagLine!(IMGS[end], pose_tags[10]) +drawTagLine!(IMGS[end], pose_tags[16]) + + +IMGS[end] + + + + + + + + + + + + + + +using TransformUtils + +# redetect tags + +detector = AprilTagDetector() + +tags = detector(img) + +pose = homography_to_pose(tags[5].H, -focal, focal, cw, ch) + + +cV = pose[1:3,4] + +bV = cfg[:bRc]*cV + + + +bearing = atan2(-bV[1], bV[3]) +# tag_det[id][:range] = [norm(tag_det[id][:pos][[1;3]]);] + + +foreach(tag->drawTagBox!(img,tag, width = 5, drawReticle = false), tags) + + +img + +# diff --git a/examples/wheeled/racecar/quickAprilTagSLAM_local.jl b/examples/wheeled/racecar/quickAprilTagSLAM_local.jl index d881c8d2d..5fcbcbce2 100644 --- a/examples/wheeled/racecar/quickAprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/quickAprilTagSLAM_local.jl @@ -1,24 +1,172 @@ # Local compute version -using Caesar -using YAML +# add more julia processes +nprocs() < 4 ? addprocs(4-nprocs()) : nothing + +using Caesar, Distributions +using YAML, JLD, HDF5 +# for drawing +using RoMEPlotting, Gadfly + + function loadConfig() cfg = Dict{Symbol,Any}() data = YAML.load(open(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cam_cal.yml"))) bRc = eval(parse("["*data["extrinsics"]["bRc"][1]*"]")) cfg[:bRc] = bRc + cfg[:intrinsics] = Dict{Symbol,Any}() + cfg[:intrinsics][:height] = data["intrinsics"]["height"] + cfg[:intrinsics][:width] = data["intrinsics"]["width"] + cfg[:intrinsics][:cam_matrix] = data["intrinsics"]["camera_matrix"] cfg end +# add AprilTag sightings from this pose +function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.3 ) + currtags = ls(fg)[2] + for lmid in keys(posetags) + @show lmsym = Symbol("l$lmid") + if !(lmsym in currtags) + addNode!(fg, lmsym, Point2) + end + ppbr = Pose2Point2BearingRange(Normal(posetags[lmid][:bearing][1],bnoise), + Normal(posetags[lmid][:range][1],rnoise)) + addFactor!(fg, [pssym;lmsym], ppbr, autoinit=false) + end + nothing +end + +function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag) + prev_pssym = Symbol("x$(prev_psid)") + new_pssym = Symbol("x$(new_psid)") + # first pose with zero prior + addNode!(fg, new_pssym, Pose2) + addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.5;0.5;0.3].^2)))) + + addApriltags!(fg, new_pssym, pose_tag_bag) + new_pssym +end + cfg = loadConfig() datafolder = ENV["HOME"]*"/data/racecar/smallloop/" @load datafolder*"tag_det_per_pose.jld" tag_bag +# tag_bag[0] + + +# Figure export folder +currdirtime = now() +imgdir = joinpath(ENV["HOME"], "Pictures", "racecarimgs", "$(currdirtime)") +mkdir(imgdir) + + +# Factor graph construction +N = 75 +fg = initfg() + +psid = 0 +pssym = Symbol("x$psid") +# first pose with zero prior +addNode!(fg, pssym, Pose2) +addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)))) + +addApriltags!(fg, pssym, tag_bag[psid]) + +writeGraphPdf(fg) + +# quick solve as sanity check +tree = wipeBuildNewTree!(fg, drawpdf=true) +inferOverTree!(fg,tree, N=N) + +# +# +# drawPosesLandms(fg) +# plotKDE(getVertKDE(fg, :x0), dims=[1;2]) +# plotKDE(getVertKDE(fg, :x0), dims=[1;2]) + +@async run(`evince bt.pdf`) + +prev_psid = 0 +# add other positions +for psid in 1:1:59 + addnextpose!(fg, prev_psid, psid, tag_bag[psid]) + # writeGraphPdf(fg) + tree = wipeBuildNewTree!(fg, drawpdf=true) + inferOverTree!(fg,tree, N=N) + + pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),30cm, 25cm),pl) + pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),30cm, 25cm),pl) + prev_psid = psid +end + + + +for i in 1:3 +tree = wipeBuildNewTree!(fg, drawpdf=true) +inferOverTree!(fg,tree, N=N) +end + +drawPosesLandms(fg,spscale=0.1, drawhist=false,xmin=-3,xmax=6,ymin=-5,ymax=2) + + +# add second position +psid = 20 +addnextpose!(fg, 10, psid, tag_bag[psid]) +writeGraphPdf(fg) +tree = wipeBuildNewTree!(fg) +inferOverTree!(fg,tree, N=N) + + +drawPosesLandms(fg) + + + + + +# debugging +# +# using IncrementalInference +# isInitialized(fg,:x0) +# IncrementalInference.doautoinit!(fg, :x0, N=N) +# +# x0l0 = ls(fg, :l0)[1] +# approxConv(fg, x0l0, :l0) +# +# +# +# fct = getVert(fg, x0l0, nt=:fnc) +# ppbr = getfnctype(fct) +# +# X0 = getVal(fg, :x0) +# L0 = getVal(fg, :l0) +# +# +# +# +# +# +# +# IncrementalInference.doautoinit!(fg, :l0, N=N) +# # IncrementalInference.doautoinit!(fg, :l1, N=N) +# +# +# isInitialized(fg, :l0) +# +# +# X1 = getVal(fg,:x0) +# +# plot(x=X1[1,:],y=X1[2,:], Geom.hexbin) +# + +# + # diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl new file mode 100644 index 000000000..348ae7e01 --- /dev/null +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -0,0 +1,113 @@ + + + + +function loadConfig() + cfg = Dict{Symbol,Any}() + data = YAML.load(open(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cam_cal.yml"))) + bRc = eval(parse("["*data["extrinsics"]["bRc"][1]*"]")) + cfg[:bRc] = bRc + cfg[:intrinsics] = Dict{Symbol,Any}() + cfg[:intrinsics][:height] = data["intrinsics"]["height"] + cfg[:intrinsics][:width] = data["intrinsics"]["width"] + cfg[:intrinsics][:cam_matrix] = data["intrinsics"]["camera_matrix"] + cfg +end + + + +function drawThickLine!(image, startpoint, endpoint, colour, thickness) + (row, col) = size(image) + x1 = startpoint.x + y1 = startpoint.y + x2 = endpoint.x + y2 = endpoint.y + + draw!(image, LineSegment(x1,y1,x2,y2), colour) + + if x1 != x2 && (y2-y1)/(x2-x1) < 1 + for tn = 1:thickness + i = tn ÷ 2 + + x1mi = x1-i + x1pi = x1+i + y1mi = y1-i + y1pi = y1+i + x2mi = x2-i + x2pi = x2+i + y2mi = y2-i + y2pi = y2+i + #clip to protect bounds + (x1mi < 1) && (x1mi = 1) + (y1mi < 1) && (y1mi = 1) + (x1pi > col) && (x1pi = col) + (y1pi > row) && (y1pi = row) + (x2mi < 1) && (x2mi = 1) + (y2mi < 1) && (y2mi = 1) + (x2pi > col) && (x2pi = col) + (y2pi > row) && (y2pi = row) + + iseven(tn) && draw!(image, LineSegment(x1,y1mi,x2,y2mi), colour) + isodd(tn) && draw!(image, LineSegment(x1,y1pi,x2,y2pi), colour) + end + else + for tn = 1:thickness + i = tn ÷ 2 + + x1mi = x1-i + x1pi = x1+i + y1mi = y1-i + y1pi = y1+i + x2mi = x2-i + x2pi = x2+i + y2mi = y2-i + y2pi = y2+i + #clip to protect bounds + (x1mi < 1) && (x1mi = 1) + (y1mi < 1) && (y1mi = 1) + (x1pi > col) && (x1pi = col) + (y1pi > row) && (y1pi = row) + (x2mi < 1) && (x2mi = 1) + (y2mi < 1) && (y2mi = 1) + (x2pi > col) && (x2pi = col) + (y2pi > row) && (y2pi = row) + + iseven(tn) && draw!(image, LineSegment(x1mi,y1,x2mi,y2), colour) + isodd(tn) && draw!(image, LineSegment(x1pi,y1,x2pi,y2), colour) + end + end +end + + + + + + +function drawTagLine!(imgl, tag_detection) + # naive + # ch, cw = round(Int, height/2), round(Int, width/2) + cw, ch = 330.4173, 196.32587 # from ZED driver config + focal = 340.97913 + imheight = 376 + + tagsize = 0.172 + + bearing0 = tag_detection[:bearing][1] + + # convert bearing to position on image + tag_u_coord = cw + focal*tan(-bearing0) + tag_u_coord = round(Int, tag_u_coord) + + c1 = Point(tag_u_coord, 1) + c2 = Point(tag_u_coord, imheight) + drawThickLine!(imgl, c1, c2, RGB{N0f8}(0.8,0.0,0.4), 10) + + nothing +end + + + + + + +# diff --git a/src/Caesar.jl b/src/Caesar.jl index 10b169e0e..702911031 100644 --- a/src/Caesar.jl +++ b/src/Caesar.jl @@ -59,7 +59,8 @@ export # passthrough RoME factor types PriorPose2, Pose2Pose2, - Pose2DPoint2DBearingRange, + Pose2DPoint2DBearingRange, # deprecated + Pose2Point2BearingRange, # deprecated # insitu component GenericInSituSystem, From 710baee333852e9a1f04aa1905ef5ebfe8a994d0 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 30 Jul 2018 13:45:00 -0400 Subject: [PATCH 03/47] get racecar results --- examples/wheeled/racecar/cam_cal.yml | 19 ++- examples/wheeled/racecar/ensureCamCal.jl | 134 +++++++++++++++--- .../racecar/quickAprilTagSLAM_local.jl | 120 +++++++--------- examples/wheeled/racecar/racecarUtils.jl | 38 ++++- src/cloudgraphs/CloudGraphIntegration.jl | 3 +- 5 files changed, 214 insertions(+), 100 deletions(-) diff --git a/examples/wheeled/racecar/cam_cal.yml b/examples/wheeled/racecar/cam_cal.yml index 374ebb0c8..e3e9d14cb 100644 --- a/examples/wheeled/racecar/cam_cal.yml +++ b/examples/wheeled/racecar/cam_cal.yml @@ -1,13 +1,20 @@ date: 2018-07-20 extrinsics: - # bV = bRc*cV bodyXYZ=FWD-PRT-OVH, cameraXYZ=STB-DWN-FWD bRc: [0 0 1; -1 0 0; 0 -1 0] + bVc: [0 0 0] + # bV = bRc*cV bodyXYZ=FWD-PRT-OVH, cameraXYZ=STB-DWN-FWD -# taken from ROS topic and ROS ZED driver process. +# taken from ROS topic and ROS ZED driver process. intrinsics: width: 672 height: 376 - camera_matrix: [340.97913 0.00000 330.41730; 0.00000 340.97913 196.32587; 0.00000 0.00000 1.00000] - distortion: [0.00000 0.00000 0.00000 0.00000 0.00000] - rectification: [1.00000 0.00000 0.00000; 0.00000 1.00000 0.00000; 0.00000 0.00000 1.00000] - projection: [340.97913 0.00000 330.41730 -0.04092; 0.00000 340.97913 196.32587 0.00000; 0.00000 0.00000 1.00000 0.00000] + cx: 327.986 + cy: 198.066 + fx: 349.982 + fy: 349.982 + k1: -0.172617 + k2: 0.0264226 + # camera_matrix: [340.97913 0.00000 330.41730; 0.00000 340.97913 196.32587; 0.00000 0.00000 1.00000] + # distortion: [0.00000 0.00000 0.00000 0.00000 0.00000] + # rectification: [1.00000 0.00000 0.00000; 0.00000 1.00000 0.00000; 0.00000 0.00000 1.00000] + # projection: [340.97913 0.00000 330.41730 -0.04092; 0.00000 340.97913 196.32587 0.00000; 0.00000 0.00000 1.00000 0.00000] diff --git a/examples/wheeled/racecar/ensureCamCal.jl b/examples/wheeled/racecar/ensureCamCal.jl index a1e16f1d8..c022889ef 100644 --- a/examples/wheeled/racecar/ensureCamCal.jl +++ b/examples/wheeled/racecar/ensureCamCal.jl @@ -6,49 +6,143 @@ using Colors using AprilTags +using PyCall +@pyimport numpy as np +@pyimport cv2 include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) cfg = loadConfig() -# lazy bad ... sorry -cw, ch = 330.4173, 196.32587 -focal = 340.97913 +# some default, probably not right +# cw, ch = 330.4173, 196.32587 +# focal = 340.97913 +# tagsize = 0.172 + +cw, ch = cfg[:intrinsics][:cx], cfg[:intrinsics][:cy] +focal = cfg[:intrinsics][:fx] tagsize = 0.172 -datafolder = ENV["HOME"]*"/data/racecar/smallloop/" + +# datafolder = ENV["HOME"]*"/data/racecar/smallloop/" +datafolder = ENV["HOME"]*"/data/racecar/thursday/" @load datafolder*"tag_det_per_pose.jld" tag_bag # tag_bag[0] -camcount = readdlm(datafolder*"images/cam-count.csv",',') +imgfolder = "newCalibrationTest3" +# camcount = readdlm(datafolder*"$(imgfolder)/cam-count.csv",',') camlookup = Dict{Int, String}() -for i in 1:size(camcount,1) - camlookup[camcount[i,1]] = strip(camcount[i,2]) +for i in 1:length(tag_bag) + camlookup[i-1] = "camera_image$(5*(i-1)).jpeg" + # camlookup[camcount[i,1]] = strip(camcount[i,2]) end -IMGS = [] - - -# pose :x0 - -img = load(datafolder*"images/left0005.jpg") -push!(IMGS, deepcopy(img)) -drawTagLine!(IMGS[1], tag_bag[0][0]) -drawTagLine!(IMGS[1], tag_bag[0][1]) -drawTagLine!(IMGS[1], tag_bag[0][16]) -drawTagLine!(IMGS[1], tag_bag[0][584]) +IMGS = [] +psid = 1 +for psid in 0:(length(tag_bag)-1) + img = load(datafolder*"$(imgfolder)/$(camlookup[psid])") + push!(IMGS, deepcopy(img)) + for (tid, vals) in tag_bag[psid] + drawTagLine!(IMGS[psid+1], vals) + end +end IMGS[1] - - +IMGS[20] +IMGS[30] +IMGS[40] +IMGS[50] +IMGS[59] +IMGS[60] +IMGS[61] +IMGS[62] +IMGS[63] +IMGS[64] +IMGS[70] +IMGS[71] +IMGS[72] +IMGS[73] +IMGS[74] +IMGS[75] +IMGS[76] +IMGS[77] +IMGS[78] +IMGS[79] +IMGS[87] +IMGS[88] +IMGS[90] +IMGS[99] +IMGS[100] +IMGS[101] +IMGS[102] +IMGS[109] +IMGS[110] +IMGS[119] +IMGS[120] +IMGS[121] +IMGS[122] +IMGS[123] +IMGS[124] +IMGS[125] +IMGS[126] +IMGS[127] +IMGS[128] +IMGS[129] +IMGS[130] +IMGS[131] +IMGS[132] +IMGS[139] +IMGS[140] +IMGS[149] +IMGS[150] +IMGS[159] +IMGS[160] +IMGS[163] + + + +tag_bag[60][18][:bearing] +tag_bag[61][18][:bearing] +tag_bag[62][18][:bearing] +tag_bag[63][18][:bearing] + +tag_bag[77] + +tag_bag[86] +tag_bag[87] +tag_bag[88] + +println("$(tag_bag[98][0][:bearing]), $(tag_bag[98][0][:range])") +println("$(tag_bag[99][0][:bearing]), $(tag_bag[99][0][:range])") +println("$(tag_bag[100][0][:bearing]), $(tag_bag[100][0][:range])") +println("$(tag_bag[101][0][:bearing]), $(tag_bag[101][0][:range])") +println("$(tag_bag[102][0][:bearing]), $(tag_bag[102][0][:range])") + +println("$(tag_bag[98][1][:bearing]), $(tag_bag[98][1][:range])") +println("$(tag_bag[99][1][:bearing]), $(tag_bag[99][1][:range])") +println("$(tag_bag[100][1][:bearing]), $(tag_bag[100][1][:range])") +println("$(tag_bag[101][1][:bearing]), $(tag_bag[101][1][:range])") +println("$(tag_bag[102][1][:bearing]), $(tag_bag[102][1][:range])") + + + +tag_bag[109] +tag_bag[110] + +tag_bag[123] + +IMGS[125] +tag_bag[124] + +tag_bag[125] # pose :x1 psid = 1 diff --git a/examples/wheeled/racecar/quickAprilTagSLAM_local.jl b/examples/wheeled/racecar/quickAprilTagSLAM_local.jl index 5fcbcbce2..517b16a5c 100644 --- a/examples/wheeled/racecar/quickAprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/quickAprilTagSLAM_local.jl @@ -1,57 +1,34 @@ -# Local compute version + # Local compute version # add more julia processes -nprocs() < 4 ? addprocs(4-nprocs()) : nothing +nprocs() < 5 ? addprocs(5-nprocs()) : nothing -using Caesar, Distributions +using Caesar, RoME, Distributions using YAML, JLD, HDF5 # for drawing using RoMEPlotting, Gadfly +# const IIF = IncrementalInference +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) -function loadConfig() - cfg = Dict{Symbol,Any}() - data = YAML.load(open(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cam_cal.yml"))) - bRc = eval(parse("["*data["extrinsics"]["bRc"][1]*"]")) - cfg[:bRc] = bRc - cfg[:intrinsics] = Dict{Symbol,Any}() - cfg[:intrinsics][:height] = data["intrinsics"]["height"] - cfg[:intrinsics][:width] = data["intrinsics"]["width"] - cfg[:intrinsics][:cam_matrix] = data["intrinsics"]["camera_matrix"] - cfg -end - -# add AprilTag sightings from this pose -function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.3 ) - currtags = ls(fg)[2] - for lmid in keys(posetags) - @show lmsym = Symbol("l$lmid") - if !(lmsym in currtags) - addNode!(fg, lmsym, Point2) - end - ppbr = Pose2Point2BearingRange(Normal(posetags[lmid][:bearing][1],bnoise), - Normal(posetags[lmid][:range][1],rnoise)) - addFactor!(fg, [pssym;lmsym], ppbr, autoinit=false) - end - nothing -end - -function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag) - prev_pssym = Symbol("x$(prev_psid)") - new_pssym = Symbol("x$(new_psid)") - # first pose with zero prior - addNode!(fg, new_pssym, Pose2) - addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.5;0.5;0.3].^2)))) - - addApriltags!(fg, new_pssym, pose_tag_bag) - new_pssym -end +# function loadConfig() +# cfg = Dict{Symbol,Any}() +# data = YAML.load(open(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cam_cal.yml"))) +# bRc = eval(parse("["*data["extrinsics"]["bRc"][1]*"]")) +# cfg[:bRc] = bRc +# cfg[:intrinsics] = Dict{Symbol,Any}() +# cfg[:intrinsics][:height] = data["intrinsics"]["height"] +# cfg[:intrinsics][:width] = data["intrinsics"]["width"] +# cfg[:intrinsics][:cam_matrix] = data["intrinsics"]["camera_matrix"] +# cfg +# end cfg = loadConfig() -datafolder = ENV["HOME"]*"/data/racecar/smallloop/" +# datafolder = ENV["HOME"]*"/data/racecar/smallloop/" +datafolder = ENV["HOME"]*"/data/racecar/thursday/" @load datafolder*"tag_det_per_pose.jld" tag_bag # tag_bag[0] @@ -81,55 +58,56 @@ tree = wipeBuildNewTree!(fg, drawpdf=true) inferOverTree!(fg,tree, N=N) # -# -# drawPosesLandms(fg) # plotKDE(getVertKDE(fg, :x0), dims=[1;2]) # plotKDE(getVertKDE(fg, :x0), dims=[1;2]) +delete!(tag_bag[61], 18) +delete!(tag_bag[77], 18) +delete!(tag_bag[86], 16) +delete!(tag_bag[110], 6) +delete!(tag_bag[123], 0) +delete!(tag_bag[124], 16) + + @async run(`evince bt.pdf`) prev_psid = 0 # add other positions -for psid in 1:1:59 +for psid in 1:1:115 #length(tag_bag) addnextpose!(fg, prev_psid, psid, tag_bag[psid]) # writeGraphPdf(fg) - tree = wipeBuildNewTree!(fg, drawpdf=true) - inferOverTree!(fg,tree, N=N) - - pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); - Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),30cm, 25cm),pl) - pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); - Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),30cm, 25cm),pl) + if psid % 25 == 0 || psid == 115 + tree = wipeBuildNewTree!(fg, drawpdf=true) + inferOverTree!(fg,tree, N=N) + + pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),30cm, 25cm),pl) + pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),30cm, 25cm),pl) + end prev_psid = psid end +IncrementalInference.savejld(fg, file="racecar_fg_$(currdirtime).jld") +# fg, = IncrementalInference.loadjld(file="30jul18_9AM.jld") +# addFactor!(fg, [:l5;:l16], Point2Point2Range(Normal(0.5,1.0))) +# ls(fg,:l5) -for i in 1:3 -tree = wipeBuildNewTree!(fg, drawpdf=true) -inferOverTree!(fg,tree, N=N) -end - -drawPosesLandms(fg,spscale=0.1, drawhist=false,xmin=-3,xmax=6,ymin=-5,ymax=2) - - - -# add second position -psid = 20 -addnextpose!(fg, 10, psid, tag_bag[psid]) +const KDE = KernelDensityEstimate -writeGraphPdf(fg) - - -tree = wipeBuildNewTree!(fg) -inferOverTree!(fg,tree, N=N) - - -drawPosesLandms(fg) +fid = open("results.csv","w") +for sym in [ls(fg)[1]...;ls(fg)[2]...] + p = getVertKDE(fg, sym) + val = string(KDE.getKDEMax(p)) + println(fid, "$sym, $(val[2:(end-1)])") +end +close(fid) +0 # debugging # diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index 348ae7e01..f147386a3 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -6,15 +6,49 @@ function loadConfig() cfg = Dict{Symbol,Any}() data = YAML.load(open(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cam_cal.yml"))) bRc = eval(parse("["*data["extrinsics"]["bRc"][1]*"]")) - cfg[:bRc] = bRc + # convert to faster symbol lookup + cfg[:extrinsics] = Dict{Symbol,Any}() + cfg[:extrinsics][:bRc] = bRc cfg[:intrinsics] = Dict{Symbol,Any}() cfg[:intrinsics][:height] = data["intrinsics"]["height"] cfg[:intrinsics][:width] = data["intrinsics"]["width"] - cfg[:intrinsics][:cam_matrix] = data["intrinsics"]["camera_matrix"] + haskey(data["intrinsics"], "camera_matrix") ? (cfg[:intrinsics][:cam_matrix] = data["intrinsics"]["camera_matrix"]) : nothing + cfg[:intrinsics][:cx] = data["intrinsics"]["cx"] + cfg[:intrinsics][:cy] = data["intrinsics"]["cy"] + cfg[:intrinsics][:fx] = data["intrinsics"]["fx"] + cfg[:intrinsics][:fy] = data["intrinsics"]["fy"] + cfg[:intrinsics][:k1] = data["intrinsics"]["k1"] + cfg[:intrinsics][:k2] = data["intrinsics"]["k2"] cfg end +# add AprilTag sightings from this pose +function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1 ) + currtags = ls(fg)[2] + for lmid in keys(posetags) + @show lmsym = Symbol("l$lmid") + if !(lmsym in currtags) + addNode!(fg, lmsym, Point2) + end + ppbr = Pose2Point2BearingRange(Normal(posetags[lmid][:bearing][1],bnoise), + Normal(posetags[lmid][:range][1],rnoise)) + addFactor!(fg, [pssym; lmsym], ppbr, autoinit=false) + end + nothing +end + +function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag) + prev_pssym = Symbol("x$(prev_psid)") + new_pssym = Symbol("x$(new_psid)") + # first pose with zero prior + addNode!(fg, new_pssym, Pose2) + addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.4;0.4;0.5].^2)))) + + addApriltags!(fg, new_pssym, pose_tag_bag) + new_pssym +end + function drawThickLine!(image, startpoint, endpoint, colour, thickness) (row, col) = size(image) diff --git a/src/cloudgraphs/CloudGraphIntegration.jl b/src/cloudgraphs/CloudGraphIntegration.jl index 5086b02cd..0adc1ebcc 100644 --- a/src/cloudgraphs/CloudGraphIntegration.jl +++ b/src/cloudgraphs/CloudGraphIntegration.jl @@ -155,11 +155,12 @@ function getfnctype(cvl::CloudGraphs.CloudVertex) return getfnctype(vert) end -function initfg(;sessionname="NA",robotname="",cloudgraph=nothing) +function initfg(;sessionname="NA",robotname="",username="",cloudgraph=nothing) # fgl = RoME.initfg(sessionname=sessionname) fgl = IncrementalInference.emptyFactorGraph() fgl.sessionname = sessionname fgl.robotname = robotname + fgl.username = username fgl.cg = cloudgraph return fgl end From 06d56d11e68be0d320e2e510ad7ac195a49a8a21 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 30 Jul 2018 20:09:42 -0400 Subject: [PATCH 04/47] expanding apriltag car examples --- .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 165 ++++++++++++++++++ examples/wheeled/racecar/cameraUtils.jl | 91 ++++++++++ examples/wheeled/racecar/ensureCamCal.jl | 3 - .../racecar/quickAprilTagSLAM_local.jl | 12 +- 4 files changed, 257 insertions(+), 14 deletions(-) create mode 100644 examples/wheeled/racecar/Img2AprilTagSLAM_local.jl create mode 100644 examples/wheeled/racecar/cameraUtils.jl diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl new file mode 100644 index 000000000..3d1eaa684 --- /dev/null +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -0,0 +1,165 @@ + # Local compute version + +# add more julia processes +nprocs() < 5 ? addprocs(5-nprocs()) : nothing + +using Caesar, RoME, Distributions +using YAML, JLD, HDF5 +using RoMEPlotting, Gadfly +using AprilTags +using Images, ImageView, ImageDraw +using CoordinateTransformations, Rotations, StaticArrays + +const KDE = KernelDensityEstimate +const IIF = IncrementalInference + +using PyCall + +@pyimport numpy as np +@pyimport cv2 + + +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) + +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cameraUtils.jl")) + + +cfg = loadConfig() + +cw, ch = cfg[:intrinsics][:cx], cfg[:intrinsics][:cy] +fx = fy = cfg[:intrinsics][:fx] +camK = [fx 0 cw; 0 fy ch; 0 0 1] +tagsize = 0.172 +k1,k2 = cfg[:intrinsics][:k1], cfg[:intrinsics][:k2] + + +datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" +# tag_bag[0] + + +imgfolder = "images" +# camcount = readdlm(datafolder*"$(imgfolder)/cam-count.csv",',') +camlookup = Dict{Int, String}() +count = -1 +for i in 175:5:370 + count += 1 + camlookup[count] = "camera_image$(i).jpeg" + # camlookup[camcount[i,1]] = strip(camcount[i,2]) +end + + + +# Figure export folder +currdirtime = now() +imgdir = joinpath(ENV["HOME"], "Pictures", "racecarimgs", "$(currdirtime)") +mkdir(imgdir) +mkdir(imgdir*"/tags") + + +# AprilTag detector +detector = AprilTagDetector() + +# extract tags from images +IMGS = [] +TAGS = [] +psid = 1 +for psid in 0:(length(camlookup)-1) + img = load(datafolder*"$(imgfolder)/$(camlookup[psid])") + tags = detector(img) + push!(TAGS, deepcopy(tags)) + push!(IMGS, deepcopy(img)) + foreach(tag->drawTagBox!(IMGS[psid+1],tag, width = 5, drawReticle = false), tags) + save(imgdir*"/tags/img_$(psid).jpg", IMGS[psid+1]) +end +# psid = 1 +# img = load(datafolder*"$(imgfolder)/$(camlookup[psid])") + +# free the detector memory +freeDetector!(detector) + + +# IMGS[1] +# TAGS[1] + +# package tag detections for all keyframes in a tag_bag +tag_bag = Dict{Int, Any}() +for psid in 0:(length(TAGS)-1) + tag_det = Dict{Int, Any}() + for tag in TAGS[psid+1] + q, tvec = getAprilTagTransform(tag, camK, k1, k2, tagsize) + tag_det[tag.id] = buildtagdict(q, tvec, tagsize) + end + tag_bag[psid] = tag_det +end +# tag_bag[0] + +# save the tag bag file for future use +@save imgdir*"/tag_det_per_pose.jld" tag_bag + + + + + +# Factor graph construction +N = 75 +fg = initfg() + +psid = 0 +pssym = Symbol("x$psid") +# first pose with zero prior +addNode!(fg, pssym, Pose2) +addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)))) + +addApriltags!(fg, pssym, tag_bag[psid]) + +# writeGraphPdf(fg) + +# quick solve as sanity check +tree = wipeBuildNewTree!(fg, drawpdf=true) +inferOverTree!(fg,tree, N=N) + + +# @async run(`evince bt.pdf`) + +prev_psid = 0 +# add other positions +maxlen = (length(tag_bag)-1) +for psid in 1:1:maxlen + addnextpose!(fg, prev_psid, psid, tag_bag[psid]) + # writeGraphPdf(fg) + if psid % 5 == 0 || psid == maxlen + tree = wipeBuildNewTree!(fg, drawpdf=true) + inferOverTree!(fg,tree, N=N) + + pl = drawPosesLandms(fg, spscale=0.1, drawhist=false)#, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),30cm, 25cm),pl) + pl = drawPosesLandms(fg, spscale=0.1)#, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),30cm, 25cm),pl) + end + prev_psid = psid +end + +IIF.savejld(fg, file=imgdir*"/racecar_fg_$(currdirtime).jld") + +# fg, = IncrementalInference.loadjld(file="30jul18_9AM.jld") +# addFactor!(fg, [:l5;:l16], Point2Point2Range(Normal(0.5,1.0))) +# ls(fg,:l5) + + + +fid = open("results.csv","w") +for sym in [ls(fg)[1]...;ls(fg)[2]...] + p = getVertKDE(fg, sym) + val = string(KDE.getKDEMax(p)) + println(fid, "$sym, $(val[2:(end-1)])") +end +close(fid) + + + + +0 + + + +# diff --git a/examples/wheeled/racecar/cameraUtils.jl b/examples/wheeled/racecar/cameraUtils.jl new file mode 100644 index 000000000..409c21fa8 --- /dev/null +++ b/examples/wheeled/racecar/cameraUtils.jl @@ -0,0 +1,91 @@ + + +function rodrigues!(Rmat::Array{Float64,2}, rvec::Vector{Float64}) + th = norm(rvec) + rotv = rvec./th + cth = cos(th) + rotv1c = rotv.*(1-cth) + + Rmat[1,1] = 0.0 + Rmat[1,2] = -rotv[3] + Rmat[1,3] = rotv[2] + Rmat[2,1] = rotv[3] + Rmat[2,2] = 0 + Rmat[2,3] = -rotv[1] + Rmat[3,1] = -rotv[2] + Rmat[3,2] = rotv[1] + Rmat[3,3] = 0 + Rmat .*= sin(th) + + Rmat[1,1] += rotv1c[1]*rotv[1] + Rmat[1,2] += rotv1c[1]*rotv[2] + Rmat[1,3] += rotv1c[1]*rotv[3] + Rmat[2,1] += rotv1c[2]*rotv[1] + Rmat[2,2] += rotv1c[2]*rotv[2] + Rmat[2,3] += rotv1c[2]*rotv[3] + Rmat[3,1] += rotv1c[3]*rotv[1] + Rmat[3,2] += rotv1c[3]*rotv[2] + Rmat[3,3] += rotv1c[3]*rotv[3] + + Rmat[1,1] += cth + Rmat[2,2] += cth + Rmat[3,3] += cth + nothing +end + +function buildtagdict(q::Rotations.Quat, + tvec::Union{Vector{Float64},StaticArrays.SArray{Tuple{3},<:Real,1,3}}, + tagsize::Float64 ) + # + onetag = Dict{Symbol, Any}() + onetag[:pos] = tvec[:] + onetag[:quat] = Float64[q.w; q.x; q.y; q.z] + onetag[:tagxy] = Float64[tagsize; tagsize] + onetag[:bearing] = [atan2(-tvec[1], tvec[3]);] + onetag[:range] = [norm(tvec[[1;3]]);] + onetag +end +function buildtagdict(q::Rotations.Quat, + tvec::CoordinateTransformations.Translation{T}, + tagsize::Float64 ) where T + buildtagdict(q, tvec.v, tagsize) +end + +function getAprilTagTransform(tag::AprilTag, + camK::Array{Float64,2}, + k1::Float64, + k2::Float64, + tagsize::Float64 ) + # + imgPts = zeros(4,2,1) + for i in 1:4, j in 1:2 + imgPts[i,j,1] = tag.p[i][j] + end + + tsz = tagsize/2.0 + objPts = zeros(4,3,1) + objPts[1,1,1] = -tsz; objPts[1,2,1] = -tsz; + objPts[2,1,1] = tsz; objPts[2,2,1] = -tsz; + objPts[3,1,1] = tsz; objPts[3,2,1] = tsz; + objPts[4,1,1] = -tsz; objPts[4,2,1] = tsz; + + distCoeffs = zeros(5) + distCoeffs[1] = k1 + distCoeffs[2] = k2 + + # Python OpenCV + ret, rvec, tvec = cv2.solvePnP(objPts, imgPts, camK, distCoeffs) + + Rmat = zeros(3,3) + rodrigues!(Rmat,rvec[:]) + q = convert(Quat, RotMatrix{3}(Rmat)) + return q, Translation(SVector(tvec...)) +end +# objPts.push_back(cv::Point3f(-s,-s, 0)); +# objPts.push_back(cv::Point3f( s,-s, 0)); +# objPts.push_back(cv::Point3f( s, s, 0)); +# objPts.push_back(cv::Point3f(-s, s, 0)); + +# objectPoints = np.random[:random]((4,3,1)) +# imagePoints = np.random[:random]((4,2,1)) +# cameraMatrix = np.eye(3) diff --git a/examples/wheeled/racecar/ensureCamCal.jl b/examples/wheeled/racecar/ensureCamCal.jl index c022889ef..e0376d2ae 100644 --- a/examples/wheeled/racecar/ensureCamCal.jl +++ b/examples/wheeled/racecar/ensureCamCal.jl @@ -6,10 +6,7 @@ using Colors using AprilTags -using PyCall -@pyimport numpy as np -@pyimport cv2 include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) diff --git a/examples/wheeled/racecar/quickAprilTagSLAM_local.jl b/examples/wheeled/racecar/quickAprilTagSLAM_local.jl index 517b16a5c..f555ccdb3 100644 --- a/examples/wheeled/racecar/quickAprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/quickAprilTagSLAM_local.jl @@ -12,17 +12,7 @@ using RoMEPlotting, Gadfly include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) -# function loadConfig() -# cfg = Dict{Symbol,Any}() -# data = YAML.load(open(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cam_cal.yml"))) -# bRc = eval(parse("["*data["extrinsics"]["bRc"][1]*"]")) -# cfg[:bRc] = bRc -# cfg[:intrinsics] = Dict{Symbol,Any}() -# cfg[:intrinsics][:height] = data["intrinsics"]["height"] -# cfg[:intrinsics][:width] = data["intrinsics"]["width"] -# cfg[:intrinsics][:cam_matrix] = data["intrinsics"]["camera_matrix"] -# cfg -# end + cfg = loadConfig() From 22498985735895c6bdbf4fb76efe511edcdbffbc Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 6 Aug 2018 20:51:17 -0400 Subject: [PATCH 05/47] april tag canonical example --- REQUIRE | 2 +- examples/apriltagserver/tag_exporter.jl | 16 +-- .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 61 +++++++-- .../wheeled/racecar/apriltag_transforms.jl | 66 +++++++++ .../racecar/apriltag_transforms_image.jl | 126 ++++++++++++++++++ examples/wheeled/racecar/cam_cal.yml | 27 ++-- examples/wheeled/racecar/cameraUtils.jl | 66 +++++++-- .../wheeled/racecar/example_tag_image.jpeg | Bin 0 -> 46864 bytes .../wheeled/racecar/example_tag_image_2.jpeg | Bin 0 -> 53265 bytes examples/wheeled/racecar/racecarUtils.jl | 68 +++++++--- 10 files changed, 368 insertions(+), 64 deletions(-) create mode 100644 examples/wheeled/racecar/apriltag_transforms.jl create mode 100644 examples/wheeled/racecar/apriltag_transforms_image.jl create mode 100644 examples/wheeled/racecar/example_tag_image.jpeg create mode 100644 examples/wheeled/racecar/example_tag_image_2.jpeg diff --git a/REQUIRE b/REQUIRE index 7e5b33cdd..714bed4a8 100644 --- a/REQUIRE +++ b/REQUIRE @@ -10,7 +10,7 @@ LibBSON Mongo 0.2.3 TransformUtils 0.1.0 Rotations -CoordinateTransformations +CoordinateTransformations 0.5.0 JLD HDF5 JSON diff --git a/examples/apriltagserver/tag_exporter.jl b/examples/apriltagserver/tag_exporter.jl index 064f129cf..0d027cda8 100644 --- a/examples/apriltagserver/tag_exporter.jl +++ b/examples/apriltagserver/tag_exporter.jl @@ -30,17 +30,17 @@ function showImage(image, tags) imageCol end -function apriltag_converter(a::AprilTag, utime::Int64) +function apriltag_converter(tag::AprilTag, utime::Int64) msg=apriltag_t(); msg.utime = utime - msg.id=a.id; - msg.familyName=a.family; - msg.hammingDistance=a.hamming; - P = reshape([a.p...;],2,4)' + msg.id=tag.id; + msg.familyName=tag.family; + msg.hammingDistance=tag.hamming; + P = reshape([tag.p...;],2,4)' msg.p=P - msg.cxy= a.c; - msg.homography= a.H; - msg.pose = homography_to_pose(a.H, -80.,80.,320.,240.) + msg.cxy= tag.c; + msg.homography= tag.H; + msg.pose = homography_to_pose(tag.H, -80.,80.,320.,240.) return msg end diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index 3d1eaa684..1aef48026 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -12,6 +12,7 @@ using CoordinateTransformations, Rotations, StaticArrays const KDE = KernelDensityEstimate const IIF = IncrementalInference +const TU = TransformUtils using PyCall @@ -30,8 +31,13 @@ cw, ch = cfg[:intrinsics][:cx], cfg[:intrinsics][:cy] fx = fy = cfg[:intrinsics][:fx] camK = [fx 0 cw; 0 fy ch; 0 0 1] tagsize = 0.172 -k1,k2 = cfg[:intrinsics][:k1], cfg[:intrinsics][:k2] +# k1,k2 = cfg[:intrinsics][:k1], cfg[:intrinsics][:k2] # makes it worse +k1, k2 = 0.0, 0.0 +# tag extrinsic rotation +Rx = RotX(-pi/2) +Rz = RotZ(-pi/2) +bTc= LinearMap(Rz) ∘ LinearMap(Rx) datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" # tag_bag[0] @@ -80,26 +86,28 @@ freeDetector!(detector) # IMGS[1] # TAGS[1] - +tvec = Translation(0.0,0,0) +q = Quat(1.0,0,0,0) # package tag detections for all keyframes in a tag_bag tag_bag = Dict{Int, Any}() for psid in 0:(length(TAGS)-1) tag_det = Dict{Int, Any}() for tag in TAGS[psid+1] q, tvec = getAprilTagTransform(tag, camK, k1, k2, tagsize) - tag_det[tag.id] = buildtagdict(q, tvec, tagsize) + cTt = tvec ∘ CoordinateTransformations.LinearMap(q) + tag_det[tag.id] = buildtagdict(cTt, q, tvec, tagsize, bTc) end tag_bag[psid] = tag_det end -# tag_bag[0] +tag_bag[0] +TAGS[1] + # save the tag bag file for future use @save imgdir*"/tag_det_per_pose.jld" tag_bag - - # Factor graph construction N = 75 fg = initfg() @@ -108,26 +116,35 @@ psid = 0 pssym = Symbol("x$psid") # first pose with zero prior addNode!(fg, pssym, Pose2) -addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)))) +# addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)))) +addFactor!(fg, [pssym], DynPose2VelocityPrior(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)), + MvNormal(zeros(2),diagm([0.01;0.01].^2)))) -addApriltags!(fg, pssym, tag_bag[psid]) +addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2) # writeGraphPdf(fg) # quick solve as sanity check tree = wipeBuildNewTree!(fg, drawpdf=true) -inferOverTree!(fg,tree, N=N) +inferOverTreeR!(fg,tree, N=N) +plotKDE(fg, :l1, dims=[3]) + +# ls(fg) +# +# val = getVal(fg, :l11) + +drawPosesLandms(fg, spscale=0.25) # @async run(`evince bt.pdf`) prev_psid = 0 # add other positions maxlen = (length(tag_bag)-1) -for psid in 1:1:maxlen - addnextpose!(fg, prev_psid, psid, tag_bag[psid]) +for psid in [5;9;13;17] #17:4:21 #maxlen + addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=DynPose2) # writeGraphPdf(fg) - if psid % 5 == 0 || psid == maxlen + if psid % 1 == 0 || psid == maxlen tree = wipeBuildNewTree!(fg, drawpdf=true) inferOverTree!(fg,tree, N=N) @@ -141,6 +158,26 @@ end IIF.savejld(fg, file=imgdir*"/racecar_fg_$(currdirtime).jld") +ls(fg, :l7) + +stuff = IIF.localProduct(fg, :l7) +plotKDE(stuff[2], levels=3, dims=[3]) + +tag_bag[28][7][:tRYc] +tag_bag[31][7][:tRYc] + + +plotKDE(fg, :l1, dims=[3]) + +getfnctype(getVert(fg, :x28l7f1, nt=:fnc)).z.μ +getfnctype(getVert(fg, :x31l7f1, nt=:fnc)).z.μ + +ls(fg, :l17) + +getfnctype(getVert(fg, :x28l17f1, nt=:fnc)).z.μ +getfnctype(getVert(fg, :x30l17f1, nt=:fnc)).z.μ + + # fg, = IncrementalInference.loadjld(file="30jul18_9AM.jld") # addFactor!(fg, [:l5;:l16], Point2Point2Range(Normal(0.5,1.0))) # ls(fg,:l5) diff --git a/examples/wheeled/racecar/apriltag_transforms.jl b/examples/wheeled/racecar/apriltag_transforms.jl new file mode 100644 index 000000000..b4ce24c19 --- /dev/null +++ b/examples/wheeled/racecar/apriltag_transforms.jl @@ -0,0 +1,66 @@ +# using LCMCore +# using CaesarLCMTypes +using Rotations, CoordinateTransformations +using MeshCat +using TransformUtils +using AprilTags + +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cameraUtils.jl")) + +vis = Visualizer() +open(vis) + +setobject!(vis["home"], Triad(1.0)) + +detector = AprilTagDetector() +# setobject!(vis["tag01"], Triad(0.2)) +# settransform!(vis, Translation(-0.5, -0.5, 0)) + +fx = fy = 80.0 +cx,cy = (320,240) +k1,k2 = (0.0,0.0) +camK = [fx 0 cx; 0 fy cy; 0 0 1] + +tagsize = 0.172 + + +function tagPicture(vis::Visualizer,msg::apriltag_t, drawdict) + # m=homography_to_pose(Array{Float64,2}(msg.homography),300,300,320,240) + m = msg.pose + tkey = "tag$(msg.id)" + if !haskey(drawdict, tkey) + setobject!(vis[tkey], Triad(0.2)) + drawdict[tkey] = 1 + end + # wTc=LinearMap(RotX(pi/2)) + # cTt=Translation(m[1:3,4]...) ∘ LinearMap(Quat(m[1:3,1:3])) + # settransform!(vis[tkey], wTc ∘ cTt) + P = Vector{Vector{Float64}}() + for i in 1:4 + push!(P, msg.P[i,:]) + end + tag = AprilTag(msg.familyName, msg.id, msg.hammingDistance, Float32(0.0), P) + T,Q, = getAprilTagTransform(tag, + camK, + k1, + k2, + tagsize ) + cTt = T ∘ LinearMap(Q) + settransform!(vis[tkey], cTt) + nothing +end + +function typed_callback(channel::String, msg::apriltag_t, drawdict) + tagPicture(vis,msg, drawdict) +end + +drawdict = Dict{String, Int}() + +lcm=LCM() +subscribe(lcm, "TEST_CHANNEL", (m,c)->typed_callback(m,c,drawdict), apriltag_t) + + + +while true + handle(lcm) +end diff --git a/examples/wheeled/racecar/apriltag_transforms_image.jl b/examples/wheeled/racecar/apriltag_transforms_image.jl new file mode 100644 index 000000000..1ecb24654 --- /dev/null +++ b/examples/wheeled/racecar/apriltag_transforms_image.jl @@ -0,0 +1,126 @@ +# tag transforms from image + +using YAML +using AprilTags, Images, ImageMagick +using Rotations, CoordinateTransformations +using StaticArrays +using MeshCat +using GeometryTypes + +using PyCall +@pyimport numpy as np +@pyimport cv2 + + +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cameraUtils.jl")) +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) + +cfg = loadConfig() + +# fx = fy = 300.0 +# cx,cy = (320,240) +k1,k2 = (0.0,0.0) +cx, cy = cfg[:intrinsics][:cx], cfg[:intrinsics][:cy] +fx = fy = cfg[:intrinsics][:fx] +# k1,k2 = cfg[:intrinsics][:k1], cfg[:intrinsics][:k2] + +camK = [fx 0 cx; 0 fy cy; 0 0 1] +tagsize = 0.172 + +# tag extrinsic rotation +Rx = RotX(-pi/2) +Rz = RotZ(-pi/2) +bTc= LinearMap(Rz) ∘ LinearMap(Rx) + +detector = AprilTagDetector() + +image = load(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","example_tag_image_4.jpeg")) +tags = detector(image) + +freeDetector!(detector) + + + +imgd = deepcopy(image) +foreach(tag->drawTagBox!(imgd,tag, width = 5, drawReticle = false), tags) +imgd + + + + + +# visualization + + +vis = Visualizer() +open(vis) + +setobject!(vis["home"], Triad(1.0)) + + + +function drawTagDetection(vis::Visualizer, tagname, Q, T, bTc, bP2t; posename=:test) + # draw tag triad + setobject!(vis[currtag], Triad(0.2)) + settransform!(vis[currtag], bTt) + + # draw ray to tag + v = vis[posename][:lines][tagname] + geometry = PointCloud( + Point.([bTt.translation[1];0], + [bTt.translation[2];0], + [0.0;0]) + ) + setobject!(v, LineSegments(geometry, LineBasicMaterial())) + settransform!(v, Translation(0.0,0,0)) + + # draw orientation tail for tag + v = vis[posename][:orient][tagname] + geometry = PointCloud( + Point.([0.1;0], + [0.0;0], + [0.0;0]) + ) + setobject!(v, LineSegments(geometry, LineBasicMaterial())) + settransform!(v, bP2t) # + nothing +end + + +tidx = 1 +currtag = "" +bTt = LinearMap(Translation(0.0,0,0) ∘ Quat(1.0,0,0,0)) +for tidx in 1:length(tags) + +currtag = "tag$(tags[tidx].id)" + +# get relative tag transform +Q,T, = getAprilTagTransform(tags[tidx], + camK, + k1, + k2, + tagsize ) +cTt = (T ∘ LinearMap(Q)) + +# tTl = LinearMap(RotY(pi/2)) ∘ LinearMap(RotX(-pi/2)) +bTt = bTc ∘ cTt # ∘ tTl + +# get Pose2Pose2 tag orientation transform +bP2t = getTagPP2(bTt) +# dt = convert(RotXYZ, bP2t.linear).theta3 + +drawTagDetection(vis, currtag, Q, T, bTc, bP2t, posename=:x5) + + +end + + + + + + + + + + +# diff --git a/examples/wheeled/racecar/cam_cal.yml b/examples/wheeled/racecar/cam_cal.yml index e3e9d14cb..3894100c8 100644 --- a/examples/wheeled/racecar/cam_cal.yml +++ b/examples/wheeled/racecar/cam_cal.yml @@ -5,16 +5,17 @@ extrinsics: # bV = bRc*cV bodyXYZ=FWD-PRT-OVH, cameraXYZ=STB-DWN-FWD # taken from ROS topic and ROS ZED driver process. -intrinsics: - width: 672 - height: 376 - cx: 327.986 - cy: 198.066 - fx: 349.982 - fy: 349.982 - k1: -0.172617 - k2: 0.0264226 - # camera_matrix: [340.97913 0.00000 330.41730; 0.00000 340.97913 196.32587; 0.00000 0.00000 1.00000] - # distortion: [0.00000 0.00000 0.00000 0.00000 0.00000] - # rectification: [1.00000 0.00000 0.00000; 0.00000 1.00000 0.00000; 0.00000 0.00000 1.00000] - # projection: [340.97913 0.00000 330.41730 -0.04092; 0.00000 340.97913 196.32587 0.00000; 0.00000 0.00000 1.00000 0.00000] +left: + intrinsics: + width: 672 + height: 376 + cx: 327.986 + cy: 198.066 + fx: 349.982 + fy: 349.982 + k1: -0.172617 + k2: 0.0264226 + # camera_matrix: [340.97913 0.00000 330.41730; 0.00000 340.97913 196.32587; 0.00000 0.00000 1.00000] + # distortion: [0.00000 0.00000 0.00000 0.00000 0.00000] + # rectification: [1.00000 0.00000 0.00000; 0.00000 1.00000 0.00000; 0.00000 0.00000 1.00000] + # projection: [340.97913 0.00000 330.41730 -0.04092; 0.00000 340.97913 196.32587 0.00000; 0.00000 0.00000 1.00000 0.00000] diff --git a/examples/wheeled/racecar/cameraUtils.jl b/examples/wheeled/racecar/cameraUtils.jl index 409c21fa8..a92a4dbf1 100644 --- a/examples/wheeled/racecar/cameraUtils.jl +++ b/examples/wheeled/racecar/cameraUtils.jl @@ -33,23 +33,35 @@ function rodrigues!(Rmat::Array{Float64,2}, rvec::Vector{Float64}) nothing end -function buildtagdict(q::Rotations.Quat, - tvec::Union{Vector{Float64},StaticArrays.SArray{Tuple{3},<:Real,1,3}}, - tagsize::Float64 ) +function buildtagdict(cTt, + Ql::Rotations.Quat, + tvec, # ::Union{Vector{Float64},StaticArrays.SArray{Tuple{3},<:Real,1,3}} + tagsize::Float64, + bTcl ) # + # @show tvec, q + # @show cTt = tvec ∘ LinearMap(Ql) + @show bTcl, cTt + @show bTt = bTcl ∘ cTt + bP2t = getTagPP2(bTt) + onetag = Dict{Symbol, Any}() - onetag[:pos] = tvec[:] - onetag[:quat] = Float64[q.w; q.x; q.y; q.z] + onetag[:pos] = tvec.translation[:] + onetag[:quat] = Float64[Ql.w; Ql.x; Ql.y; Ql.z] onetag[:tagxy] = Float64[tagsize; tagsize] - onetag[:bearing] = [atan2(-tvec[1], tvec[3]);] - onetag[:range] = [norm(tvec[[1;3]]);] + onetag[:bearing] = [atan2(-tvec.translation[1], tvec.translation[3]);] + onetag[:range] = [norm(tvec.translation[[1;3]]);] + # onetag[:tRYc] = convert(RotXYZ, q).theta2 + onetag[:bP2t] = bP2t onetag end -function buildtagdict(q::Rotations.Quat, - tvec::CoordinateTransformations.Translation{T}, - tagsize::Float64 ) where T - buildtagdict(q, tvec.v, tagsize) -end +# function buildtagdict(cTt, +# q::Rotations.Quat, +# tvec::CoordinateTransformations.Translation{T}, +# tagsize::Float64, +# bTcl ) where T +# buildtagdict(cTt, q, tvec, tagsize, bTcl ) +# end function getAprilTagTransform(tag::AprilTag, camK::Array{Float64,2}, @@ -79,7 +91,7 @@ function getAprilTagTransform(tag::AprilTag, Rmat = zeros(3,3) rodrigues!(Rmat,rvec[:]) q = convert(Quat, RotMatrix{3}(Rmat)) - return q, Translation(SVector(tvec...)) + return q, Translation(SVector(tvec...)), camK end # objPts.push_back(cv::Point3f(-s,-s, 0)); # objPts.push_back(cv::Point3f( s,-s, 0)); @@ -89,3 +101,31 @@ end # objectPoints = np.random[:random]((4,3,1)) # imagePoints = np.random[:random]((4,2,1)) # cameraMatrix = np.eye(3) + +function getAprilTagTransform(tag::AprilTag; + fx::Float64=100.0, + fy::Float64=100.0, + cx::Float64=320.0, + cy::Float64=240.0, + k1::Float64=0.0, + k2::Float64=0.0, + tagsize::Float64=0.172 ) + camK = [fx 0 cx; 0 fy cy; 0 0 1] + getAprilTagTransform(tag, + camK, + k1, + k2, + tagsize ) +end + + +# get Pose2Pose2 tag orientation transform +function getTagPP2(bTt) + @show bTt + cVz = LinearMap(Quat(bTt.linear))([1.0;0;0]) + wYt = atan2(cVz[2],cVz[1]) + Translation(bTt.translation[1],bTt.translation[2],0) ∘ LinearMap(RotZ(wYt)) +end + + +# diff --git a/examples/wheeled/racecar/example_tag_image.jpeg b/examples/wheeled/racecar/example_tag_image.jpeg new file mode 100644 index 0000000000000000000000000000000000000000..d246944763b504e2fa7d0a9c66c89ec120513fc4 GIT binary patch literal 46864 zcmbTdWmFtb)IK@ z_+LUs`Y%U8Mn*zLML|XVpA!xJ4JsNs8Y=1=%s1#5|E1R}EKCfn|8D-P_JrnVp+o zSp2iJyan6d+1=a!dvJL1@AB&U<`#Z;{~s;{0Mh@$dd>eIalL!Rh4|kXp#6sn0ny|2 zM0$se@}2_~PfQu@n-e|_XW$zG@r3-^UUU$b$~n;3c^ZR|mV1lt;y-BrgY5quu%Q26 z$o?;2|2M8>02UI$>);{11Be3ds>#qs8EK5ka(88&BIBGWumgkzsaj~P*f@C*;=Dt* z&F=blRJfe7r}sYYT4|HgODB?lS5Sn+1Io)Kp9Wmki}@^8%83L=z5->G8TGABxSF>T_ z$Ac^=c{OoyJ1QV|v>x#r%Qx|)g~DYuGpAhd%E*2^i%il}vB^F$a(tOgLe^Du&GG#` zSk)L?kJBPBm421?V=s_y{yg^G`){)gc||ox9xILmRR{#RRu=YN!}99PKD(*LqpK!G zTFq{5_FPXN81e8^k2I*I7idO;)(#6+ndnb4xSa8csv{;?oSq);nGR z((Mk-l1lD?y>_{Wq^CrEVx-;uxFY#m=JZ{<4x2MP^g`d5Mr!B^lTG|mmA0=$F@0Nid+M)MzqgSLHGRUaJp%787&9NV~qL1E!71>s`N%H zRbGWZzKDLsN87|ZK<1t~o{3xiPyI0uq#`kcPR0$dm4(?j2H&hO*s*rIh{wN&arWR{t`BxXA9^& zLJ~`Q;jcWwZKcM^FoE?Ryxq=VdIhR}UH& zO^o0aAFWdpJb!`Fa=a><`zM=RuYrNcL+IlSLgX5jQmVj(V*q2b=`nU>fsJzuTJ`Vw z5enKkzDGuMu=CIH=-(+gfMF!y#35&jxunqkLerVqGOouXyC5YxYaF`ijAm9!inYuU{*85Kd57u;I27mueswGK-vsmjsZK%2zvVi zV@mbB+HLzjH3wH3OONrouM=F~Rs{?=?5@24bk=A6jdq-- zy!Sg~sC9?jtID0YbxG>v%SFsbXi8?-9$RaaC{x5WwaFNQ|ENef8at|zOu6VYk?PqV zu7B11b6sht^QrFnY@t%jaSFh;Ws!=F!e1aU5ImBOAMQq0}=nkWXW zE_3DsVP#5*m4Z#o#hLFuu94CSm{R_cAfZ+ZIPb{RV@vW~@GiKYzW7<>>Lm3S;s# z)wxKHHPc)eQQBhnYEH!>ozwS48e4ft+Dy=rwA#8f{B}L3fPa-OXs<<5O_-`xtf@F| z?ToEW7;JYiszJ95%U$HRiLC-WTWKIF9d}iGu?fwP;cTv)+@JCUD1EUg3JEe=UyqtW z)z%pZ96?UK*X3D#BE+{>6&y7jF(tvOChD~xk+Dcks;Obocmh`h!|{F#u%ti8TAJgl zf^&W5oVK_e3>no$!!(%KbnQ!vK8SAv~L)Q*UP z&D9gEK8Xc+-V~44e!P>O06XJ2-OZ#$@qy3?bhc^Y!-owzts#%27a@A)knz#cPliKf zoI(ALPmJ0$zobg?H3TyoSaAlJ3jg_8v$Js^IR4=)=|L*K7E*ElQ2S6M5A6?JuV~yY zBeUD^Xem8Slp;$A-$%A+l&V|*0DoA|C^AQB8BTA%oPqq&7tfFpjBTw0di%;N=^0Onh4IdepZI>!g=GbV;W8_viDr%697SBZS-XExHRScD zk85P5x3V09(KyYMd6XI1#^oHpOY)YoNpt&d5p_M7f0M$TEO+9& z-nR|pq3(C8gq<^AA04S(KS{p;Y`Jy+p;7pi7kZG<;%Hs(#j23D&K+EmpL>cl-1Ts5 zZiD4yf12CfZIe_~5$psx`dRDD#Y9(#+g+>1B9I$b$&@3vyyf4yr7trG3s#PqGoq3$ zff(EgL?-g_l`FT}m=s2zDdH;YZA9C5v=$)M?lm3LYeRsy*T7MDokp6f<-d=B9yEv@ z@t+w%EV&TVN&aE_C~ctq%vVV z0quhp?RNrgsc8Guh*nGZ9!}4UDS4*pB^t*z8hz)&iQC90*Od+=sDx4Pg>lIrR2x_o zOl!XmXS+BTBq(bF*q%mEbl<)UvRC940Wr;iOPmG;=3l1 zmLvJv?|Ce%hywEzpIIH%^%5d|3&HMMx@R9P`1y}iK2n)^Fd&yzLO(^?attptR5@lk z_4{q!ywrEe><*4B~X zfRdE%#h+s3#pM@e^l^~Y_#I+a#4(deD!9|-E!CU3Fc0x=6aTV6zKm_mC^F!`Q&QE~ z&9xe0ERuGhbO4-xh8xkx!2$+b0j@>;6KnwtVgx@He^Esd+*|E z20{WaB9Y*>EidH7jh2p1Rs?+bRon#u0QH~Y^|16@dfY{irg$a`f9Ub{S) zcaELLDLq2-Egz{=IAyYV7 zAV;R^&~h*1%7-f!m)wJ3M$z$)=uD~%@CQF}>9X|j1=VO5!7WTN-~U`#*jg>AO3At! zD;zA1MQj1HOyXIHFs%d#48*Lrve@ydp6O(Eyx z%*C+bVhLQ)XVA7U?jb?sKGVl)Z0}ArSu5P_BO@HC4P4%y$t75uYQkN8t2$8cUn1-0k$yD8$zCFM0z8fP@bpn_J` zsv_XH*{A(Zz2^7tp??)jtS+*AQVq{sfdw{#oHAsTQnEV9JomaSxwm@3vzhOeI6<_J zy!l|Fo1)SDP?NjBL#xz8gN>=ixl+}UW2--CtNO&>BhYB0)mu(Npq2gP#$|eTM1pS~ zN}}hYe4ICj?K4DWy%GFa^Fr`&t)moLa9Qec0-YAh@7h8X2`!K*3kc%5bPRxBW%>9Y z%PL}r3Y$qnHZv<|p*PMB4x3=|B!WXlY<`wRH5Ofjv|=W*nmYwUbP-o0u4yPRt$dKY ziz%e+uNQ<0Yq(y~GT~qOe)fX!_2Q%Y#pj#?>bRl0)S(0^1Eti0Apf8Y+8_g^A`q7= zNJ!nLJtRiE+2o>q@|0>NnOx9^rcRnlJFOJ3s0`8x%jQ~Jp``|-LgxTqao%WP~ zAHjEvtd@n*bavc?!4qiE>giiQ_+>f(5m;f)<3lSL=iMK8&9+c>WWaV*_6;GS>x<(fpre_Fj*D#6O7=Icx>P@7-yG;DxMqb-!(sKwEaQ~Be*QZ@B9f^(n0`dY zH#q6xd@P_Zxxi10A#v(GSua?80R(JS3$af$04$Z61W73=TDYttc`XoI1KzH3Ea>Gp zdQRBFIZd8&dRM5;Te}HHk1D0hdbZ1MARt0LWrm-D-3t&a&ZqKg0{1~P3CH3Mn-=Gd@k|S2Gpzy)Tru@Cwz-uS`_~V*?z&SQQhk<_{Du}1077ZJBnp$i2ZD5 zlIyFMKthQ<@QhftI<6rGMOKmRSsT;X-rtJbmY?sndRz9_d-gUXD^iZ7%y*QTJdBSmO%ncBPl*+!!Yp0S}*DeJ|^za!}T<%=9%p1%)cGlnMynLn}` za3?gU_mf-!-Mw-jNc7vWF4frzwPC6&6y>UpbqDP4?A_x5719-)f@GViY%-?-q`T~^ zevlqvT_KI@g0e=>;Um4OdQiXf(C1hmO?7TUQ)g+c4)Q={!}pf`6(*4tHyusujSj9V zI)toHa`QG=dS)$U1|J>@YqlG>@UnPPQ3g2q)FQ#R{}00K1KGc+2UeD^735%FXe;N-(pCcI>7^aZHnCcd*QC z6ITD;co7fkb)??_lMgS+;(8l^g)xc7qFuWfsHuJ{6#4^+k51Ja(K2)OU=S;?Zsu?+sf|NuO>b$4MWlm{~hj>Ks(>Ov6l?Y1@p|uKydyQ9Dwc zoivk~7)Byb9x1f$f2P=Yt|^i_fh$EaC^TN`6m4FCo{YBy&6BRWV;w{=mz`l};D_Z} zQ5z^aGGR~0B0L@#Cx2LxsYs40cBL;c?lI-1%qX^q&V?!;g&m!#9CIj7t%JE0k$i{G z%X=fqgJlhDW@K>ivf1K3Ilh}SKrOJ*VfXF^)-g#E_CxQ4VMr;Zj%CPZ5hL;!C zU=@7TJ2GYG8-F4VztH@*(!vfQA15U*q+cBhi=A>2pQp7?>*I!sn>2L80Q<2i6va0! zDg#Ehs2Si{ox>FpT;Vrh9t6tW_pKbV*|nSVS?zOmvr+l|+q)!Xr&oX3%<-_rHEQZypULNjdACUgC&>GfGEmJnPoC zCILOqt~o^Ak$n4K($4s(Nt3kebwi2d`6-SVS+Rk!b(lrjsFG8_AF#DgOJWV#NV$_) zTPLcM?x7KPnk1DD;mw1Ie1W%u>YB{vq@%b<*nwX;Po>xlJQ z-1{x@fJr}Q1G$>0Fk>Y}8>^+iAes|sH+pcKiNWBO7fSUH5E}BE%iPHh`;)-d%fPb< z@2;QD6I+&la68h8tC)_~ILqFyFfn*(-C2v#O;DM^+~_$X6w!+F5rwid{w^+kaq$sh(2ic;C0pOIa-?u6{^pgr&p z*dB*<^f3amrh=J$YPd2QV6VpaoAu0S{o9X{*m|sUGkEg7ycmUuZzf#*Nz`q)-jk=o zT4#+X4-uw{T<_&kr$WNqO}kjd+kqAjG*3F*6vwkJ<89 zn$Z2=`}0GdqWI<4U*;BnryG(!n*2Uz{xrI9S?V&pPjdA+5{9}2&3)hyj=V`L95^Q=Ip+F$h#$eC{q>ZI?~$n+}`&h2S zr`<{aZ7?PwUA(>}dwX3DGm@+c`S^E=;-;$}+U86WvXV96_r!E|1D;KMeU8{5>!H*X zKHV3=Ht`$}u2u=0E(J~ef!NOFW*ts;)_ zA(0Dh{>T$ktE}wMZYh-s2}Mh>6eEX_He#*~XHg6BGv&3o*OD1aN%XAp9CCfT8XKmw z5=oWM(3?IbewXNlv~0Xf+D)7vK-B?7x<*=rYnnEz!N9S~Nl32Zwfm$A3q6l5{!Bd^ zIrGVFMW!~MZ&%9No4*XTN?%n8LWdO_+T06*XOW1(0jdo$dB&drSh37Mow!|qB)|r% zc!!^280U&g*24>@9mgO_u54AiQv;rU9=}fOoP3uqmtp^uY2rT%+1e zvKuCETO7tGYG)f7AE{>Ax^uxr$;wLN?GoAivXwK1(Uf`l{nS0=wx}VegvIEYta>3Y zQJt`Vs}(6A(NNB%?{{w=x%3@l#_4LWLRe$bMML>ogir_75$4ae=6-ngjtp1XTpm#= zU81hE1F&XJF?${QGN;SPQX+bHkTRW@Wdpo_LkT=AC%}ZmK`Q=-ibzfS;i$N&Qlm^67pmNNcz|A8(nyqK80?9h@&c&ht9bnIxBzc`5QX<|LK?W>twQyJM(le2 z;DJwvTInuq{C!xNPqgsR2Yz1;ctQ0Gpic_c?lYVVK#AQAa(bS0(lH=?$`Zk~vJ{0y z)U~O<0CFoFWgaCg*a)&WT3K-pg)C60*2Rw>*J^%uNL1)X#R&Ebg#1$%(h+_#$AwIeG$G1cobUIVdoO^{j!$5Hob^+eO9GPn$h*feHcyP~=GLoL9?MG| zoFa%xQRh5ruBBN#`N^Hv7MEQe9AvzxuFlaZvFmUl8^g@XPwJp*OjK@D$caX zNFPkF{)iMWeE4rIsgzsQ!iqd|Y;IYXtmRKM_R@q-M?w^2`RZHryq1IW?%-Mn9x-cH zwnb#1z1gL5gs^pST5ze%O&Gt~sMA7m$|e0gd9d<_@Yz{5;90b(No0HJCwo={I?>OW zlhYzL!5qXi4fYnE(?6v*owS_=R~u{loATXC)*HTs|5@Gcbj$RwMlC2t>cAsH%L%Ky z@*%Uc-uo;6b4ro_x^e{y7R4GrAJFPqJ++>n4f$o6^FwN@tiF7Ll%sDa=|J0nD|yXY zj@Y4XbuI_kL3pET?QBFyg4SrJwJK6ueVRn)vD?X^)ut#2^uyUu;#ktAwHh9o&}eOc ze?E~-88Sz-cz_tXw$f}pWhb*P?qQ1mIf88$B5tORRfzTK47i%${HFw$fFzKSAVvPu zm)sl%#8+<{By3f5H+VYF2QuseN5VrF!och%&5dmjlsU-3-!l$Q^yrJ6-SlO()ht+? zbf4+xC1SG^SaJocehb$w)IYv2h728*Nl`ZT6E7XQl$e{1*}2%~m`H_A=^S)mCx%Iq zTR?QPl)|*4?Scwj0yVgO-njmC|I&iJT&|*k$^igU{-B;a%)5?CD5_er>zJq#6!%Ts6 zS796ADLMt70nw&HNTU<^szqz&Mn~UlW9#RnmC~jjYS;dY{duG*lV?3ha=RkCl7<9E zZhjA4T@z;3xL;?^hY|}jW88r3xEO6&RU&I*AY@-b=-qLjHLm%^!fU?iO(vBhxB~2p zoAwx)OlbFhsr|-V*5eOHIf#t)Y1U1Ni~U&|6285RD_g`8{2->i@t5&~9$Wl`iYoT9 z{Mrgc*El~>Z2o)cwH&(rH=G(llj1r0qFL?<$-^ZrtJy>xv{T(jX3JT>i2a~Lo{42v zsWA#-m&}n1*81hOtXKOjT;Y7yf4|ip>&fjiBznBU@tk~y?`fkWAZIXDs7Eu@%5w1* z#kzp4cLHs$m#EiBk}?=v=HQ;DVZ?@M+*;IzipT5YR@PuAHt?*nbNUxW6J)Y0`PfY9 zmJppC?=w&Ev+BL+up)n1>X=rd3zmS{*y%wXswh-#nJCEDBEW}MqYF5T7yKtqBrYj1=TQ19h!-I; zQf@buo_fc7PLgdeE9I<&mihZ9)sbI+BW$CR5~An5=s9~fDSsrvXCZr+C*!8Bz-eSO zc2@$$-nx(8#tp6#hO7~w< zp#(q)Zb2g!PVuD0A%{E?zOwcClL`JW`Glyxf1E%qz1c|BOWza&X0$Bn>2w)T)(Ti2E4wr}~-Hyo~)O@xOcjlljBzA112oYjv8DJWAD?>~u> z9z^jMA1mRb%Gd5>n(a+fPLB+G2d!&!YJyo#8l{J9exV844tCUPvc8877n$XzD&(1< z&9sq#222CvAm6Ot?H%2Q7-e7RyN;Xsq0$cV1~54*jtJQEEA%^pk?T=t-%VOEq#v(F zb45RcFNi%w++5bn=!ol2yO%C+T;TZIxnT=k@8>%(ZNR3^*?M$aX3 zORXUteES&__kkadrqS}-jS5$PW6FM4pOq~|7xc8HN7?>rAm`wukor&OsExMU`K?jo zRO!GY)iKbGQ5?NzrII$+nQm0eNwP%oXAC2!hsGBGMrqO^rTGC->@F(v<*?cdAn2ch zwTGw2R#-|Y!Xqz)g(ZOQxe*GFd&VreP|}Ea0n|T}>K<62S=F|!BD??sDQ$l7yS5Xz zrI`AcXvFc+oN;5ULF%z@y~SBvM7M4Cl1I-)!#X`Y%MMTSj~6v2HC1Gz*G>0p zS2{g~yGmNR#zmzHghVQS!!j{BfmZjw08adr-}XM0xdqQFjBVe|YwxA3a3&61v$$j~ z^D$YXI_zzH9sWBdnixW~=v^fiS^|MHin z0Hc(&kxXI8_dk2xDC*Mu0Nf(~@E1TYpu=c^lKz6|2W=Wj3oZMijhaTks`Bq51<+N& zP1+1j5VqLXSyZ!KXC-6?lx^pVnp@_pIe+_h z3Zs^sibYfEJ|o&@TS{=7h2bCPLPlxH@)s+59}{64#m&?x`@Z#-D$K779bV;%?6y@W z7*t<+sbZ{kttpu!*J%?E@W(zap2#$pbKwbQUv# zyPJbUX@czep)n=}*51BHZcbjj>0^`Q#+BVMk6#ueY%?21V^%z57syG}tG!D$zqBkw z`;&UyB@4+|dDxgd<(ic!5i=mOAY*yE7JC|gP95cPW3dxokzFjG!+1InVF{hyK9c%lsZ9u+2J*v1RP)8lRxG-Laf*$8xN z81jSB^4G)&pi`qFW6H(~C_c8#}@`{dcDoT&i=;p(eKLdw%tOMp61LDp}x21&- zp6R1+NGoe?69;eZ6Hz>JC|hG$IVVT?)qdu^h5TL&_Z7b&(_LjPN}gd7rDBc`_-Xlw zvgojP;jzTfnJ8UA7M?J&(sK>o-OFaY7UC{1JK1sHglFam)Rm0b+~v%}vNF!xFn#zW zr2$k!ZTfiJwKdYvee%Wkh3jlZ)eRRr<=N5h2l^Bf=h6}De8&?0J}R6lwP)2nCSMMD z4&I|GXU|v2FeCsv&)8e4Ln?0Pfh9K^h_?wL-@m*1PyWYqSoGR(c&SH`!P|W zD=sV-rgBo1igajL1U9nQuY$9Ggz+P*kL(BpL~JME;(h$3Y!xY`HXDy4g}g|#Eo{`d zqFYT(**H7py1~V(KnOtx<%od0d zOK4XB>jX(Gn09DH$+e4~>efD|%zxgBBhX_?_XU@dU%ddn1e{k_9(M?D`f=RqSZYAM zO{uw$I?d?X^#$buFXMslRg`v^=YAln2T&Nck^}j=5)F1l@%IdI9%(LaT77_Zp2jz{ z?j)&l?idBa3Vyts0l_(B=lefSjxB1MavXEJlKXI!w)J_E^5HNExqy16Puu$*#$;6D z%!RYQgg3SJiC%iQtDeAPqZUoxeyRv;4jQZATET&=OIY}Yol zUI%2W zs_VW`Kg!#b*g62p+~+VCrz%Sdsu(>ng#Uw{EQa@zTfqvg#*XwyQh23UzrbXJiY) zu0b|*{VJN`%MVk6v+eiLnm_l#-Kr-!-7^Z%b0>enL9fk*)xr-hRK~U@Q#J*o2s)B! zCO8xkt_R&&$gLnW3BlDGx+A&Hn5`aXoIFyUzD^IO^4#6wh4yrXBea?K|0Fry{JUF? zT)K~59DV^bv)I1clw(LgMHr$CRj6+ozOtcOcgEEtnCBwZFOH&hekXgI+#NzeZz7yO z0v!fxUs-0zF%F?f6`e)8H}NYkcMCa+ibu&elJV$&I%MjEY@{)NZb1}gd^TZSQjz4? z=u}Shgmp9J0ftG6h*RlXD8H~2(BjNx3N%^bkh4mGtdaYfo}?Y##5#Y{iKPdRJ}c5+W64+E2LxUS}q$zwTU8fMDCw*0iZQ>ily|EKiNvHx zin#oDtsG(It@E?8E8wqXk(SrFmOcEZWRyn%cQE`}?_sYi{UWsdunS|(^@ar3UJ~0^ z^Z^l`f7MMTS5j}MO&r^D$b9MpX(rqQ_TrV0-!C3`NhtaX;f`}f`j1i2-XHp7_^%qg zGemvP2?62z=-{*)oj#XszqWRz-|h zFo*xd40=X4gkkeaKb2h_VH*#v!nSP{rb6Z_r@F9nLX-^0D_~@c}Ahp74 zt2-<+<;vEP6xnb`8a!uZShIg(SeYW-g$fU%c6u0Li7>2E%Y52uIUf$rab8#_p4}GC zWHvM0VU|;JDVD?=8JFu>4hhtet&_y5OcF)xVt($+ZQ$*l!fcLywA%`FFp5g^Sgq_D zc(>5u-$&av2Vn*NW2?fW?zv*=mAb56XNNQThdr}BCA#<3-%ynyy~cdE&|eucqSpsuWZ6 z=xgvdK@mu{S~?C-awjYD3wQw_8(x^eeQsjYBLQxV{aF4zZGxZ+mOd2i#Wh{VnpkTZrPxK$B(93!o$T(dJ+C&CYbd z7Ufe38wI|@GH3rrB;205xbp{Z#&tG_ZW7tp8o6KRT?FOfui^y5}U~bu< zW{@GTW&Gx*skf@!fZJzlTs->UKKO>+5wzh zhBM6^_;#PSMjVVR>ji*a0coRZ?TE+r26q2AJ#iefHxYN1POILr&W*BjZJ5>J*Vd3S zY*R$9o;j~|3M<)j+i5+`*k2}!#7aZ|R*y2t6}dL&8l$Xj8oRWsvA?4St8=YJTN5kw z>mt*p3#Up+aEZM3!$8&-0nr%Qk)T05T>14H`B_O9PRS(Y0%_i>_K-*RH`dN~fFW!} zRFPoJ7P)LO=ZXeR-R%kMm3}k(jCjyBaB$sMdbZJ>=X|9U!_!4B1x-*GgPl-&wI>bl z1Z(UTMaY`;gIQO`TM-exOc-~@);w!QrhX#hme?UP@4#Dq$r!rM6*Er|eXJV!aKS{y zvi@p;7)?;FCz3Whi$HU<@we}(e^>_GdVvFQ9^IBbEBaP*Y7Y~AiKqJm0np)Zfr#vH zT$7f&V&kgtP}qZ?%fQ956m=pqxgC>0g5FKRg`jqvUUQsn= zD5R_{dJcB#F6efSdkFRXgF2qazD80~gbSm%QK47UiqM&^4L|}XJV~zf&VfV;Cp$M` zJcOry6q{M9>_0#W>fd-0SUKBcv{Zx^HkWnS!=S~ZhRVOMIKyzzo?ikh=)*I zMxkrag2E$uJBHhVG|Yzm11%@*jMG9aCH|hmj!W*CPdx~4*0|*L#Z20W`A7DOn5{$P z09i6~1!nsFG)=x@JcO8Q!GKr^i$nXklBiG+bL=&E9OAWgZPb(U)%)S=_C!nb>L1eZ z%rQrY&HaB>xGJqO?4}7mjaJ$=TXvMZ>8~4Sz%j)u*y4Bd8=%sP5)`Mz!sGX}7pe!O z1w}oQqHKQFKVjesZr|Kpbx@mGg4VmmM%RDuuLK}g{kklN2-HpqGqt3u0RF%ZeC9G%wdDqjGbO@`P) zsMx2$=dsuZ$2pYSUq7gMV_`fFX4&dg<>AqN0+am4a{RcX-pMOBckRJ(;B7ZCi^H?h zGxZo){VSAsM89CdyZ*{h{{;~ILxjNqB$4kwmC4%5|7W7C{oNhJ>+E$t*3#X`8ws+k zpuIP>21FND5_IC*dKU(T`ufr^L@eddi;0tsj+#_#+_9C0q2VB%i9BiL<(>p zWb@CxZ5{u5?jJkUA4ui$bWpkp19}w$YzzqmSzZ8mje-*2f>Ia90ryS@KGn|)0Sy)r zOQ{drnh~Qp<=~7@Pj4y=8$6CvF6wU#!WNXGmK9=ZJ3u23*k?aarbapRi2A#6n`1fx z3OxxKzc?S>`io0@GcGGC?dnJpEQ%H34OLw5!t5dRCvq^QaEt3paM@(Q#UZrqf<1iS zcGWXUIr;XIz0hHA{$6y89sb_nr%Q>L4=gwQU3V6-ano!5<~b1S2+!&w&=c0GKx5 zM1_{mH1NS+mIn0A_y^6rV)pryn9+gJH%EpI>zGf$-6k;%efMrRN#`nKT)NC@nJT++ z)!eI_ZTRDH{o2Q8M(?YROTOcWa*2=R7E5@u9~)Niq&=v>6-`3{LMr$v2i|dbDWkS032BIu2S62AKN>^#|LW#ISzL858+y`Ux3%C-=dC~ z%+$^*y4cb&B=Q4ik-tPW>HJuJYf&Z4WXK`T0TOc3I+wj^rs3pg$jzZ2J+$@gr6j7-9Kw=X~qq;Q7D#v4o#n%LK7+6TD6&|794nF4DKeu;liq4c-LxN&Vr_nLm~;LP+e$W5U8n=6 z3C=q$G*$^U0?I9f6x8z6d3PDIG%*R7FFgc`gMMq*h98uY)SGQ-- zI-J}_^bJd6DQh#gY)B2o$3N90KI)eN&M}u3mSU@m%UhX=BKn03C4X;T1FR@F7v$d7 zgEr?X2rN#eyo9%{tY=~1>wsgV2kS$s#^*SmqOt=#Z6Q#4dKv$acd%j1I_XpPvmm@% zjx;UQ|J_k$Xh-mF=ECo`+|h1H(aq=$3=z&pmU*@le-NBZW%^_n!-QyI@~0SDDWqJC zc=Dax=i|8Lddawf-1@VKhPAAwKY}QaZ>sm(2W}h+ZFEv1vB-Ru5QC40x(m+8;C3A{ zn`^rLO_RlPN0O18Yn(R!ZU;oq#iwGQ*)Ez&DmQG-mzE6HWmp_+M+cbky#2wE*7q3| zprZ{qtx^>2Vt|E>oQLBO@#yspYFAm(m`Qu5@8tvP^XTjB_&9F!3@+hTO(2p-{Fb+m zAV`w=PZx`4n)-QMzn|*hJ+WcKp8>u;bKg?oJUX2h*LK^3(}5Af zQGri3pJ5&+st@A6M)l7V@PrEwy?Ka~obFN0Uy*E#Enl#=Yd49d#|1@~Vy*2RMJ-ci zv*Op7jA>#SR;2^ozUBP6*8sm^!H!JPr+|ONz`Nbo!Izy*`U1Zwur6&O`eM&_@NaR| zus-M2IgQTHtAuOKayOZQxAVaMx(4og-5e0q``2HwaZA|y-6e^8?ei_#BlAxEpK9aj zD}Llwt~WfsUCp%xY4&gI`Ae@^Kl@&+N$$$>ICma)aNZ@HGI{B_D0DO=4@IMqTvexx z&D;+-tUjzxzPqo?)g|1y_z|A5s$D@m?XTj67P*G~jqZ zTE1*Zb7%Xpw>)iN%n--0Ou$<_hMK|EbJFIjlDEI?&K@LV`OQic=!Pw71_>9BUM*Gv z&uRN=wKo_oht?5~W;0z+cnMhE$8`2;C37)C&A`7cok?Ir`C~YZtBJKkw`{M@Ny-9P zDvh9V4N}T>^}gQ5j}y99?9mqLg+z;biw#Lm<8y)& zVmiI@3xkf=X1LL>idlAQsK3>!H>T6zXUp8SGBN*}wwH;%i|Tx{xnE@JIdi%&Ua>MD zS98o)Mt=+`I{3Y!-7w&&s5Dc#v1&DYU|)`IrpJ_&tsygC*jlIc?M@3fUG<8?>0^R9 zzxvMoY(2@S0bEo$n;SMGiM$`ECiTOj*%-S%)(d)IkniE-dSgtct5-_#=}1fG0RqXf zH$exvB7dDm1W3eAM}>{BN!cj9kp#J$au?+)Y2Fe4igxgHT*4vqjBhz;3v!PNsx z4*mfT90OSAH!Tu(dM55D7gRPDjUK1;=D}JgkY(a*L^LF@)R$&EdGJ74BDQ+w%(m*> zI^PCU;cZov^lX+=$oB$<79c!_PRHViyRt0F(?*GKQPRRdNp@-OT8pKRI~G-mq}-7p zQg}Ohfhhd6WWGvca;@g=KbwdubLA44R>52)_F!GmVPP3!>xb$R{5zHRt2l3jZAzvM zKmvl za@Yk*OyBjp^e7vDMb>212B^*`%(sdyQrINr5Hi#>|=GPLtEc3 zQqyn1OPPoPXWD%*a+B8-Bff0sNP*kobM0ldwdaP zPbyb*->!8r{PhAj|Afw+RgDUt!Tzk&kYnPHZ0BSf+k@SSi4+M}z3H3={T8(f09r-% zGyA^s;Z80|ucqJ(izg=~&ja{}7Ru)+liiesgF(8hEIwqQhoP=IB1a)Bs-`1~Omc~m zmFhe>S1PEQ?ZqkJ7t-Ii7>_e;ujnq0GKXsIzU4ns<&3bgsR+-kNj@OGMc-cT7V{_c zob&wz_bBr0vqfh=QIVnyDHUC)_Z`X5FWBmav#%$XouBpT6nc)Q^^kj;F7E&6J@PZ2 zg;fPt>R5TmQ+Hy~%DGcJO6mD5p79RG1-%*}UL9Sl7XYvW+pKD3iRZ}DL6zypLanRe zQt*xQ`9K7H=((oYD<2zrv5Y--1(vm9RR9U&wD%I3@AoV#j4@)mY0}!?c&(Q7;S?OUj?&x|0g53)Cyhp-?CW|}lR=9I^!qnXhHr-ucrNWw zx^XXx2(w5%L|L9GkFX2jmtW>hZ0+XqsRG@APl#{~6Swo>eh-`5Tn$Df%pC ziSh${W~{1`{{!nl6u)f8@R4ix@Ah=n?tDq`JHwiaA(4NxB19@Pz*=$!(ef|+7dJ;s zuZFs2yy_3xumgH=UzvXob${$n9BA6aa$%#rh-W^m1%9o6!EJOagZn8ivpfuk?OPLa zE9JBJN?2JQ z?o9b{v1iJ2Ykz5%Uc&&)!V{Fk$*w^NjVQml6X=sZV zEpUk!ko`=?!^^MJljemp=_0Eyd_a?EtONaq(dtlVhorI3w zm5HTXI#1>9>BVYZPGh%b&uVHKlHH6;$v)kwDwW7nioY}>I8|0W91NPLr$U8{&VFs# zt7|WrCc<(E;MN=d zGm{{;0L;K=Iqy@zthX2=>MFLH58EgXXNEO!zobr~U zUb&7Utl6$+`1zAD>^fG?xpYH_qV(-pcY&ZDV9s-0wCS^wtdFTKqc}JjtPNRNT=U0n zE2SunkQ*l*Ggw;WIAh4jug-AtvGuOTCC%Ggm6&i38hSuMKfpDp8W@f6$@-00len zgq|w>ov%E&w-_zS&*Pf>LAEk!-Yg2^0^a`s<_GIv&^PT%peKrc3u^jdUD&gb;~tA& zfnN|ThL!P4UDMnZ@-+fS=xgsVzX|ZOy^q_^_$HpMrfZ+GW|?(oX&TAp4bF*zNaDVO zn)czaCYg>p0&DS)_EY}=f~)xV_G9>V{{RoPj}IHTwx23rl_u8(5e>Jtux0>E$4~D={Yw;fc0K_`Zx4J!Y z^j$iindXg4XX}jC(524D7`AjjABLYF#?5p48saIfO~V-Gfx|0udy3>*LP^gwF3yO! z?TUDX+4dwX#~JxoHQfl<>Vzypm%I)T(xk8Dw zp#^Yx6sEK^wukS>{1czWg6R`BPki_`@{Y<=|AvN4G?&T_ItXVCgW;_hqrq9Y}y>q zmWQR5&)MQ&-*kR)=`)FJBb5mxuTpBmIIoJquivNUewmDqH3=^xE5wk0Z|iaC)^lx|TnLEix}n!T7x@1G!lpm% zr-DH@`m-GQIQQbPt#v8wb-{BQxM-y%BqN~)y35arcRKWI7OQo-MByEqHTq5(Ugn47 z*o67oW;LCJ_A_jrEV1{kG}0lGAolQp&D7O-@9iCyLw2Rl`&Fl>>b83WNhD|o`$D^; zvB>u|^aZxFwb;{wf4F*s&1&o344%(Nk6)JZJ(Doo4&0OY*B_|r_7W1&T#SLiR~46U z<1JPzpC@dR$D9&n$OGw`tp%yq%Xc1=5S0|ME5e4XlIi1}O*BhlVU@Wjt_4xmd`8pR z<$zPSu6kD&sbA^_{M^~zJWtu9Y#J4S?Yu{(#o<}MvrY0%A`h8>BB<&5xpGn|x%5+9 z?TzH}7JKWNC31ddcgZ!UqTFeAx59a*MN{{;KS}`Yv^&eAYyOg?WA2)@rs$SFDb~fk z?;<7KtNrh4z<(HNF$Gkr-!Ygz6X4FY^U=zxj)6rJHHLdrpvC_t;C2!%94Sa z=R7U1Y14UHO||S2N|WzzDf7lncv;d@!A*G{##hdN!5#yf~Su%zx@qO)~5 z(n}K}V3Ekds~T)$bLsk=PwIAIK8#t^l96(LFT?o$X;NrI}$i~ttw}>r;-GjnD@l&_!Nmxk~ zd@+0Wn_(F|sI3{?A|2BYGUaaKJ~ z_7B#)kJ=_!t*v-UYRJUC7U!h5K zBfC=1j1X(}ZX0djAiH1lKQ7`ZHJQIeeh`1bMs&B)e{EkIGsZ$odKxk9it%A|8;*v) znEwERzG$)NpS88^%14j3F5*ucj?jOte8Cl^v0k_Rv->J|KU(YN>FM9m7QY3rGA!g9vGSVzxBmcwYWRXJU-m%owUW!b?3SS$ zo}3C(TA9fs=rMLkn`y-i~jOA9P79H@DP)zdzp|ieMdEv z;*u$De&K$@_cCg~vu1&)DJONLWAa*W?XRY_-@z-X4l;z`k6On60D@!cr&0ZlJUqabR z$q6kx@66 z%)~M2PSUVuXqa(~j2h=i>ND<)mfDgs5EPz+>st3Std>L;##XX*qCVb$5-@rZ?^ds_ zl07h#IbegD)-WONGTUr6uOQ^reLqkyF?d2OuSD*)WM1!Ebl{bE6PX3{AK1Ar@=o?k4I8wVY$c&0LbM<<$^_!hGB z2kBAUgsF_b&11KAaw?6pc`iX1uE<3kv$0QBbX&5g zoK|L{K|r9dC#mgPH_sfmMnj6q)mW^H8OG9mE4q|!aa5JAeO4F~kO)4tm9Id>8cgFo z2(2x_k0z>X0DYg1Yx4{}&#`AGcDv##{YOgrzy1lg;}0_R{5>;*1FlEsUn<_gyCHLv z-o1DJ3I71%8FZhGM$F1WNwXt9=DxQjpC%<=sq^{uB|Hqgzs(LVuiHrizo6MXtDin5!QT^)%o_YL{{VuO_*{5*_L$Z# zLf}g$J4XkTU!vh;oOC}nmCrHwaqxfQH^p1QWAOXIm-?i={>ce^epU2W?C<*le$^fb z_?2Vv3sUe+#htC6Gpz8kxa1z=*w*L#5rfB5Xg?MoN=wJzIcJ4!bWzcxpD^)K0P z_QCi^`&;OjI`6`1?xnZ2EF^2lOl`r(OjqL{?fIj=h5K0eh6a%BIAAmC&Hfej_xuoR z#!qc)t@wGSRP!Z%52it`XA)cy`Ummj;SQDY-{H;Q!+mb`?d`77mE=K_f-r04zYTxE z4S#2CF5%|YJV!31C47~*h=M;6UufOU9hhk*kSfLrW<3-N^X+T)#{HE%A*nu@;=kGp z!dxNypSKJ5xpi_p&YXXDTO6od9>@Tcsx@zP%fcpBN0PCLA^Nh6Ls*F*mR z1efsdfqYy0K|!N<=SGhCTrrI#0QX;Ct#u#pSr3bU27WR8Qnv9I!cQJpYp9CCN$g)M z$sWSIoBj!{;%PiN`&#&M8e$gV*-j2Q;<{r|*xoI)e%syr4){-_=sKpGVes=u2yJ8v zA5DajfP3xDehq%wJ`%R@AMF{g=oU7@2#w^e97?zfHy!{L{R!$brOcD;#@T}pz}MlY z{1wl`G5BNlyqRK%g}I7HCml{l_|)q69A2l_pYU)00E>;^!tWCJc3F?^=W?L`0M|wT z02=!v_OH-~h<*Zij?gCK9fLT{escc+!3Mlz{h8xEZ^F+Z3vk84{wrUnzZG=<0PSCe zz9q5)@9AOaD~jLMzq$n-F}REzUJ9lR@Z<=aaFgM^uvX~EmhD`6H3veuF(QP#37 zZN`zVYFbMGr7t7{jDH!e7>t(p3hdeZeXH}iCr*Cp`t2S{(6c!$L*eE5*5rmC5nM+T zoP+>iXX`}23$EdDv&2(Adu~R)dD#d0eyqc55bKlD22_{#pvf4`&;AnHPiTG_HNKFtycQ- z=Iam2k360$sJyd^WRBbnjl(9oSuWtxEhCGd2rvPz3fx1P%W90Zl4Za1?Ho1Nz|RP2 z-zv*=PkhX3C_W5$R!Md#pPspV*Hd?>$#*staHpVZTRA+tuyesawafjzn=ko#{_|y$ z<%xd@eaAU{@E^k_)t=qu1P@w|K=>=+iS+rdF0FU8WZG9h~J*he2l!~UdM#D_G zN8mO{Kb=G39MSJphxR^Q{{TMG=lT!9KMLxagF^O7RYpkKFcOjw>g+WWzb@Rxj>UTe|J%Y3p3Ivz&cs^2(!S)5~ylo#6zFz7i%q9|IgQ z=QWM2U0hx2W+)^eXB-;S@VnbV%*w z6V5lkRb$B%{d-f6%G(mT4cJw{#Qewk*D2$xlGmvUdS;T1%G~I5YtgA``vYgUO0%TO zQUeTxwajt+5${1F~{+$*5fwpmw zRAUvBtp~Q$qq&S_T#ePaXEMzKus_4=S|r4A&|F>V*G+2daHpZJlStG&*mk}%>}xB= zaHYKIB7NrTRdg*uM4=h5MK>2>mZ!7b9n^rb{2WubL z{{S3(F(_pocBBxJZ~(!t(d+4qQz+_}eG{ zBoQ=fz>jfXrSQv33CjEb0Fn8>6hhe8zU%crCjS87y1FRSe`%cp?GUQn>5n1DbA?RD_4$JQSeykX#f?E5_yL3MV&X-8oi5Y6{V>?`R{5r4sE zJ^*-fL8fV%9JjMrD28-vji0?v7-XCH+>v&OgKh`to~I?G6c{9@rO;w!k}iO)X5wv9y4)RR6wZg@2X*u*k2zwn$? z@&HditwTIz;dN3DRN{&25|h;ZgZ}`6N$URqZvB>Y%`7n{-FYAI1g~}Y>1c1h7wSKG78lqn->9r&c93|8$R+d0?^NWy zX;4I4uWG-og__vzJ@~9^NDP-DPfob5h@|!*?r6)t-oU6mYb_L&-tCi~=Ct)mBM2f@ z#z#06o=^zz3F*@nqC`lvs3o&F?hh5r-y;i#0eAG;$^ zL9#LD?*m2DxVBZ@fO}Kpc-jCuaa%)k3XH~v$gJ$!I-ky{YnHx95~>B~wRA!y4sg7B z)eU~yIc*6GlDy*;-A$a6MqQ+vt$Ab9y;pJtJQ*W+KTh!k zQ7Uv1++!BCGEs}&8~*?iekAzA;=hWn zJZa;-MK1LVkM*-GK>_5RpN)NE{{RGC@sQCzGRvXMCKl#K3IXGw?Ozz73^#4BM*WyR zJ?ek6Z^WBlh+1X3Ol{=eVsLP3Crv$qP*%|&&|b4_F5$Vi-JS40Py7Ve;8*R%YpQ5J zwD*cErM#EQd*)OiujY`uf}bAIVY1{@9eLyj}3m&J{Z-WLV+)E z&*@%%PS(jJ=hCIKyq3!4Z!IJ-%^PMp7pk9X>2&UmtlB@LUkUi*!nb}Ku+%R6W2Hyu zK#W@BQH?Rc>$!z~O8)@CaiZ|u&+RLxYTg|27N4jovvDMvb?kGhlefAOJ!|EQ{{W5u z02eho5vJ-MI=-6SMiwa;5C`R4)y?JQ{N{V6-pB78Vx!qK3Y)q0Kl~Bn#tq@`+Mdry zX?{!4eo{J|r}$UxFN+{qyg#ehAUJzhTzc2wxBL^w#DDOQe%Kxtx=BQ8(iLuf#eVnP z6u-8TNd`s4eFtzwc{zkzfz?X!KOFug>8Hd002?*!CPA}p+b|ziH5J5+`&_O9oaVZJ z+S5v17xt#ttYcC);koIIn&q7fLR6o;G4!v_YsIOv^?bgnmKqPpp{ht;Q7+&IwN+$DfFkoBf5x`4hAur$=!~~(YC)A5=Ps&Q^~DuYV=!I!Uz%#>NM5{0(C1IY@~$6kU%}w6-2~?8yvc{#rX!(0HH0Zz8OIC$$gg zUz+6pC22QTR*P+JTzO}E1wF7uahCdZ%!*{Qh_GI*>0eQn@Wvk9)>nxBDR=?x8rsvv7VdM^O?W=36oC|Zq_OHZWK{>U zjE&CNHR`Shz;5QxCGmzW?F4)Le;GUnrs=C_x~O6}$Q4e1iry2M45L(2$3lA7g2@<8 zNd~OxwsKy|EnzO3 zn%9ahZLDO*)6RSQkIK9nD;e}XU|QVD42t+*KZd$rhWDRj@k_s$PWCy;`qzz`aK2xM zr72Ur(mfmoeT&4^QJs%@H!1=7hB4PQ$=^!1{w7u09aM9Q(Z1DI+RV)+Km)j}p9#Y? z-lF=4@B_ws*TKq?rz;<0N__FTEx7*9*25|gfw{$WI)oBLkf0iu~E}O2r&V7lk-8+)ve&l%1Y`zNDUQn0X^6#E^FDqlIJeq|E6eSjs`Xm1U1S8dj&A*HE>0|_3fE0hgQ~Yc67D;17cLW7h zC5?V&f58m(lW*fc7U^M#^Z6YA0J9x_ldv<#HL8Ml#L9moSjo$nhq>h*G5x(iWsd-A z*E%1G{v)(^HjYx_C~Vq7t?9w9gTLUb{{Xdbz@OSy=Ru3%2Dx{B+GafWxw2?+!3P=Z z*1mB4sNtT=_LK3H4LqD{54Yz%2o=E-Bfd<8=eaetIQ2A)QWczF?#(yv22}&6G`x;z zA-L!Ct)#uvMY;NC{{RFi*OKGF9xbtwLzv%z{{RAve_H!<#%z}!3DvDrlO>yv(!V=@ z;D^2=I(_emG!|e!(#&?}ugt&UU!j_o>x&zkVmEJvh6jEPeEwq&Xf$-tl2sEwJO0?# z-XXrz8l#cILD7JTQsYOq}zRUzg&v zZ2dL)>fe=ZU7IFj%!%N$+EtnzFnXs_TrWam0(auIH<4Z z%82~1+*1Y0LjY+wUpxN*!DGAbE3|+=+2agbxWneX zQ}!kC8tZ=vEi4Sdj3{{bz^?D&ufnekd}Q#{z7z44s?BEO43EggexHMyc4y>xxGGg; z7tsEAU{cQLFha;iLHDb+ntkQjD`#gcr~a8CZ{=ULdN=$MqxLP*vUPu=qE{vg7Rj6xAf$c=Gi5Tlp<$ z>7NI_Dr<6iI(Nfw5J*YK{DG$f^DAGy`VYW=ggzVt`y0TPaFywV2Op($cA8F~rwx`i z7U-k+MY{h0g;%zSdt>l-!{4z_?H}TXMAJSSUa5Y9<`<9ot#=wP{1Th?)7OfPJK@!s z$r$ssK)=ja?U?d`2kg*{4x$zR0160zkbI8ohI@`{=@z$B*q{5qt056YLHS=_H!6YiG++(Q*z9StN#V(x*f z1)Reu&iui;!=u}i0FNM%C1Tc@pb@oHkuXqCoMkb8Q;l>aakPcge`@T~xP;08|VxEAmWIeCO$u9pD}$L~Avb zdD?SSV7p6ojpLUY9N^X6QdOTyZP@vjishxgc++n6WXH$Ztz zaDD4~5%-L$>}%_In@5vi1nkHkj%)8v*)vo+*T9`3X^u=W>?gPR;=T#DkWZ$0?s}o> zYwaJ{CsjI^?DW`^m^BwnXWgsp_Ct7m$Mvt# z@UHMj!oP^Ltl0oiL-frmN1i~>(9(#cC*)FnyVq&~4tC&*Va{+s_o>#>`cMRRb2ri! zKhmdz!+Pz*h@jE09hmSE(awKL8bPIxPyLVo0BYT1_DS(&*NSv)4Rr%`)*}0Ga0m3S zc-BAQwO<;pJjpaq4o75M5V+dgb6+m#f3x?-FA%C7FTgs3hsM&@>^(h2b=n{78~asw ziTtf6;Z?(v-R4#Pb(@4bnKc`pZ}9`hI*-JE8ejP5TYTPXGnr>SGEHNchBBb_ucI`t z_$7bsm8i1)tKq8Jq3Z8_8OPz8?mR#L00eXR>3E9z_OTtAINXX=vHH_a5j0YbkB*E? znA!>aX~9%uc0oNxYwwK%{s^6Ua^P!!8l<-4keYC6>hz!ZBGYknddNkODUVCgid+86-Cs$wzC{o#e~iFUCKHd%Ds!rm=XAj(t<%9nQ@WE zYpM}PG}^IR;|()>!Vio>LU>rc?yn%V>qoF{mT|2h_+5h+nUI`G2y)AA-yq*^(do;tI+!Z6$9nZUs`Z* zM+fUn18>R>d7~&!(_f8JUh((xS1D_I3Kk@AE0om~Ttd)fWRA7d>fi(_c^LZFH++E$ z6=CaxUd9~8qR{&{{t2z(Wz>8>ccmE>j0J|=^czRLeKKVEj!5g+SH&OjOuc*Tek#;- z_9S_Qv5)tDmG+U4Aw2i5(lXkd)F98wv$%3&W{K6#%uhw^Q6yG1wCi^an#dpz&8H+(@%ulUe2+`I=dbPV|brm)GqudcX8l(W4OE#6K={k z4314`YPxm4wQkl|c__e!0Mr^zm#6q*+Dltxbc~~56P`%-qUdPmzh`e7c!S3JPN6@F zolV`Wn^`vFZ#czz7?kwsUS;rq!_)W&QPw2XwK#U_7Yy;Qa0_G#^cbh}z;%}(`cZO= zs~{N3A6j^SN5|0BGD40`Ib7q0{At)_*kn~WFOyGcM+6+!bKJT}7e9?n9K!>9emSm8 zL~d4530N|+J9{%3=4wtd6 z<7i8C&MF{AyFW2aB7T(8E=v$QrPYfO3k**G^Dt6($$AhfN*`QF=jy~U4RGPrPOcU0UR+$ zy-oq=rBS?;v#0f$L*1>fj2E+noggg=0)@n*M70hbBSX|}&O?LO1e2j-U9<}E4TRKScd3ogI=DWMu z9Rzk<)%pit5X1_$pw4rY^^G+bw7tiEFJ1+kKn4Y~eO^?dmoE-gX zISG}rDWIHSwS7!}6XzSVE=3HFASlCcek(<#NQM;S=~?=8vG3a>;8p1gvX&U*-jucl zEl8T-w-9ANO6auNKF8wQ65sOD6MzpswaDB*?{kxm_^zfl1I3zTpm4VNACa#cG>Uk6 zFD82U3G&yAUD4?J1Wi7*<~hJbt9sS@!*pX@kUH~Jd>bd)E@!%S>B5iBm&9`IxMu?y z75Nq>?q}-QW$qgdclMnG6>Q;sed`iv9^?YC^gQ5IuMFNK*2d0e!0ZicC8SX{)KTx= zxm&T*dmQZ6mljI5E(je#9gTEaR;st!Qpy3|xKs6}>lV?^rAZp7ECxkocv|Ejk{}Kb zMa@e}LX)}E+}$HsmS)By!ThU;48~&2KZ~iZpfPI+BW}1j=D9nmnkPBzdR0O#Y(aWN zJQY#Wf(dYCA4-P9R#`v*bKDx5NqooMjy(X^yQIu=O>M;Pl*wKPOx@Ob;(kts9_$IcdX81#@X#kV6Q)aMATv$kFAuc15xH==?ekaEWHM_P>c7bOJ|m}m6MdTh058<~W8#N{ zZ2ltrIoCcN-Og>(S+RR#a5?h#e-=Hf;=c@k!3};DXjY4Fto%)y{{U}O_C`)W17CIA zxhE!}c>}I1^jtir7$fs}x6FK@qJP04zhvz!0jy{jH~V_x_C(L?U3P{400h(hiaa_M ztbPuX>(2RgDIY(bd*M~voaZ$YuwIG{b*hqNU7m5Ge#)P-=Z5k9i}2Rc5IP3=znHF{ zPViraG<-*eElVUx6Ofj|x8KyLfTMEPqY|R_KV14LE3_JVOVYZwQb4Ury zss_>8p`J!yNf^yL86PiNa|J1kfI#4yPhqXfZ5Uy}>)SPROfIqj!;Y0z!v;CQKBBBy zyCUSbAlDUDp|n}?=k0l5mfsX-bsy_VB>LBtS;#Wbx$9oh`)}JVuf&U210G^mhdixi zb{z)<*XJ3G6l=+OpQmLnn+r|(BITvbYF-CYF`QLMF4RcT7AKx5(_Iw0Sru`fxvGyQ z%|tVUhV-vFBikdmmF;f?s@xM-Zm!j>xR7)2RdtJYWe%!-wVsb~7%R5`@t$hq6EjA} z6;fmh2|eh)WtZe^bm@wGF-Xv+(muGT=YihrF6@8-IISYAM44G(HqE^UUZSOSHvTI5_<4vYzKOn{2?I@IezPh!fjwVO`54(lbcd&q0dQ)ORDYmLO!}vTn}gARc{9 zdK4SE!&zv3j>z%9?*~2Us;C&8j=kw*x)U~esPd|QZ2MQ^bp9js7E{*2c$;S&es#-U zpD9>5=bQ@aHHKkq%sKV0O6iM8#O1ozsY7!K?0X;V7vc}>Pl{sNbAfQ=DBbN}p*Ii= zhyL=ZAC-Pze$D#l_7}#TCu5KvOb|~`*1t|t_w82DNR5zSPC?_Kuh4RKrH4o6{7Vj8 zRo7HVcMNTQop_}4NFWj_Jd&p_aZyUjr@!N0NVYyiwdW}tcT^kjXAYwJcQIHx969)I5n1h3Kg>cE7V`t%y4U2ZUbWvQzUHbo=#{ScQaw| z&5UlZE1634X%uF)plIMgh}a&L3-42nhw`SRxXGO+LXR7-Hc z<-kxsiOvV>QC#bq6}o2cUa^bU%(_wiYQVP~s>dd*CU#{-b2ncRz8dLgI)}uK4&08u zZlxIhwVQkUO8(DUG7+f!Zt&6(*lLK!^fk#?+L~qu?Z2Zgt@}6YjOmjFyD&KE#ePzF zIrF6=^dw^+iLcV{+CTQe{h4*|gL?0UtUfDgx0hE|#UYB`aK(-fO8m;ur}AD^jC`g; zw?3K8e70u>-Ost<$x0O3YpK&A4$7eOjzv!mxW0ws01dtKRA9M}c-%SL$2G5}OB{BN z&A=xl`d7@e`Q`PGjP%H|flyBw+FtXyiAmp2P*xDlQS zG@9H(I2Hw9GuIU+qQ!Q%*9FjJMm}n(eqr3key(&JQ&Xp(3%^tIw`! zJr;zqsc=>>gE&6*>mCs<-W-k{+yRbj$bj-xay|L4QTTs&^Y~q4BZfUade<#ajKZ?K z4o}9AeT!d|gN1NN9=unYUD^wsPGK70nGP1cZ{lT{lIGg(Fah1Pn)1C$abx0%%x{vC zWM;oY;rSWv_qjA1C*KA{NK~n2;NNueoi|;ckDIZGma|E3>k5dN!0CbRyQd?Y&J4KG&ROe?Pb~SIqdM`h0zg!Z2Kzmo@ z*tC^aey@X_>0E}!S@ikQ=1gLD$~tgK58 zGHZfqNbOGNLh{8lET9lbBDuXWKAMEN>yz6M#q!iq6&) zjfh=dM~#p0*Db6^EEBppBj)4Yx(IGVD&0rXmyw#Q;w>a<7GG+JVU%QjD_Cerfv2*? zbG`WE*0&%LGH*E^mCoC=f?x>7)6H+dRiXgDLDIFhfzY>YB8CHgNHv|}c|*FVe1p&9 zS70g!6_?_+DR8cX_$x|Da}hm=B)KwJgS#E8-M`?MI*hV>WAO%>#4K`Yg@-*z4PQN* zhWjqoKQ4NHb?jg8O^r#kkJ=*ilk(hY-d^8ahOeZ`EU{5%&u5Wxz)$7*qwk>X2@Ioy z*EJN(sM!bWnsv);VgS!-t2475q>B9(gKnAmbx)0tEx*17Dk-H^%7Qvm7`h&n8xX*; z>FZk%8}d#KQ`PN`rDY|O$+LRkws{phFvs4-Qr08BdnP($;yJQ5u^AtgQf|O<+W!E< zAGP=sQt+%glg#%xl^MIT;DzVukH);#XYqfGZG1i8IeaHAz1EF*-)fPh8+^XaUbgx_ zj4l&3-T?kmMEPIjAl` zl@e7yFypN|O}bl)V-HfG=8`?Etr1X2>5B4Cf0>r1V>}7~`uQ3(`IeKEp{Y+Ks9yH2vl>E$rVL@dz|MZHEqad19{`NYpN*Zo}@AzuYuaB z-_CyPxZ|J|Zfst7NKGmp->-F^f|C5Hs@n(v{yGMmt);9Xz)6XGR?tc3pRKucrX zSLv66{v-H0Ujo?bnwO2VxC=tNg~j?I91giP`FnC;i2T?b@JXzn5?kNv@O_q9!$dg> z$`A9ev&yk>!@|-&Q#Z<`m`OD4WA#H({i1#c=&Hw4__?5_-l26AWBMB9HQ(Ed_G{8h z1Mv#h;~(?Fb>_bD3{t99HJZU*LvEE&fco1ACKb>-VulyCm;WnJCG~a_>Air*-%he!Ie>(V__(xB- zApMO&$7B^OdIgkUGR1>Z^NJk~h;YOg#M$+I_xu(*GDy+CXU#@0?)mzhOn*aITEF}i z*W%5}vuJ+}p3^^cG|Jn5D*40vE-}xRIP?`cgZE?PPzP`;3Fi;&*y@K3;MVD$!K{D5 zO@C@FWg2}yQGGpo6bNzo*DtO9-9NU5xejc;HEOnMPaaCfKV7xKO=hygwGjSQYAYyX zflx5cIT)^KXH~9tLx-?S>=yMO+E4bt)l`i`_Jr{Z7Cip>fB2f_^=}{id)9$nXX56s zau43~q*MO@vewJP8n^9$o^#$E^8W$Z&j12yj=t-nn%P#)_EgX@~Ksc&bmMf*)?eoSGX13%Ea7S}ma9cuR8wNbi*rzSJZaGdDp#r!u7@5>s zo%Ar!r?ClFTygk{yt=8k(4;`QYJl@5K<(2NO8)W{jK&`Xk-@JrQMNQsfZ`aqS6*mIblFB^ z8SBsrh8PzZkXEjnJ4mdukVxxP=eK5=ww^QEzRsS<%1zmiO9)JRkbV@?3$$b^9uK8h zv4N!d2|4wtWQu4`#P#Qjscb@Nnb!tLVm@H$T5}b+w6SQ0f6`a2L466DG8u92O99z! zZkhYD=N08+lT_P193Hh&JKa8iw(2q4h2!|M>0J%&^4mqU$aiCmbgpl}fJ=WZ)12*0 z16FV4l53Izlevxn9`*TFGM_D5AFAMG%}F+EX&TtMm&#@&=N^=5c2e3+_K4TyEu4gO-V*-+Xlb7~ErsT|B_7A5tRxp`@hj)Itv^#l)%4c4Mg~r+kywKK zK!!EQ=f7&Vr0N9Ew6+27nw~}}38ZQaxUq0S=mG6pmXoxR8+ky$80N5b{VdvDuBi(Q z5(ydUiqNvT-5hr(10xJ7lu0tLEt1z!lp^uFrfSSn$Bcu{Itq5T3deNLR1*Axde&QC zc`^@}0o-P)+6O~=*oQd_)84cETBFL2R^!v&ou=NU#Cw#Va7`zOkJ@ZDy6~qt^s1$# zh@Qtcbvq;+cR0pt(?4Q;RWxso`dr85H!_IH_3K_yc2>XZsMuC2N|jLDTLFs$c?qR1TU>q zd16pZvd16&bdWFeuhM8c(?2Pwz44^Bai6Ut6)I{ObxkS~MB3(_nEw6rWBHoL)V?J6 z6H0O~yl18&_uF`l{EcmNiBXf;8w`Pl-1P5IxhVW+6~pS^wNLElqDW%Z{wLc+SMP0! zn;(^Oo+1ALf`NX?IyUI-J}O6g_dqtkU&76WW?V5>$2H}4S z`O9Aa0D_?W7O_&=7l&?j5;}Qz3Y)%}u1{0{0D`XA7G={u1!_QaNhQ=`Bk~5N;wZ;? z8^{s(#;} zwN{`{_I37$Zk+=E0Ana`%r#*w!wVhcZyci?FS32uu+GPHQL!G>{{SZQ$|bGe9=mJv zuUGx6e`&uEF&dwWp^rU&%#5GN*Eg#CSNPqix~=cTjb0UV(hKxOhO3S?a zpNM+I%H!tqZrQ6lSJAcG+kNwEmop6H`G+~KI+?y1tr^!29<29~_22FBQ z!+tN*bqOOQa@pN0k@dxVcb@S`51oK-DK220;MBY}~{O>rsn5t%qW z#d%za_4$eH?-nv(^K0MAc<#RUr zKveE;*0J>oqg2{*cgk!1b=`N1s^wz8iqp4A|$rO&}P?@Okf9 z@kj~d@#d`guFI9r75OY5BAdW_BSjySt)^@z0+ZM-84lAmj zgf(-PpLpYN>&-zbahkI>(s@zWtwke(z+fDn)#y{3OLAOSLLm&SI6P70h`Kq=P9;ye z20p#2E1artBa`V}DxsT2Y23F3jX*rZj&o8vZ6p;n8*(yxcB@ktD8De{ij`GFV{y-* zsNxAA*jE(n0Hbdl)$*IG3;K#|lgOYPkVR+AX=|44t)#b5Rk=RY>#aW8^aZ%>>58Ov z8<^L5IrLgfT~gv@V>;|V!UuXuyM&{1X=92hcB3DVU}BNhAiIJ5sxvX$x!3}IDj_ol zXxlg^sH<#ASX_xD*a`fqo%W9R8i0@jQ;wBp8Hkg40Q1PJX>)BOVO(d9Gg!@lcedq8Bn$&mK`_Sb{{RkaO<6p}Wm#HN3C|;*Yqt1dZRO~bTjx7pHOZBU z*v!q_fGe!{SwEd&a2WDrtSd*U6PvNGu1Ft3oD32pKb?4vuOdl&@`k|-cpqB!PY}!Y zTWO(lfa+_+Zds(%XB%(>^{=Vnsal-Znm#9p?&?AM{)UE?eT#yQe)U}~7C2#=r9X%D zO)}OkTKdVOhF|q^u*PazRa^F0;+q-cC$)X8J&%v6@ftQWLlTm}eQGO<^z#W(k)A2@ zY5xFVQRYe;K8B!Mbdh&1I@P_5_8)h17=Sx3ZUtuD-ITRfM#&Nk4Dnhw+776iQPs=` z?ECib?VnGrWL^l;*cLfEtAiO7=`+y5%GFWnzYg6OLVKs`=cngd{u#3u`jm|_hEEip3nXX?3q{;wZIYif4OHkT|b5G}VO z0x!x+UkGZdW}&O3_9)x+YHO=Psk$BSzenV~m>Jg8uVKn$KL* z!ApxN1h6;&;fLW_dgZvb)iTOsoKUTayH`~#Y&bj@S%G2 zuWA|HXhzANL&>ntS5ZeZacARy5=viR@jcbSJxP&=@~ETuc*0HcUtFGf{{TsTe_HHg zgdyh`AFWLsLDxG*D=Fs~nSG;ZWpp9Y9F%?qwYO6uODQ08nHc{7TFj5bws#k1J%NsK zk}JQSB}iF<=Y!2)=ytK)UJ^zKCy*;I?F#bVM#~FIJ0m&#DPw5{@oo+|ARwi((=8>t zNYVfRJu%1YT_&w$(pk3DBaXF$q`4Y`!M7f{?L)>*t;<+=zK29?B3OV#9XO~Bl8o+L z`WnyzZYwEShl&IT_6(v1n9oBOR$EXhGs2en%kkxU7h-WQRLgjCAI= zb(?j#vqnO5wz1&TUSdOn06f;I5iQAVC7SwBkCF#-TOg_t9~i3Ec9B{{SyX*-L~{p~ z*tY|vRjy>@Se9%`5kT9wu4*N4vak#Y#ZgPrqW)Z4LNUX1O86Bd@J*6P`n)a~kyE z{*RE0%Bj*5g70+rqgGhJ-9Pn$dnz@v$XX^K_k{I#R6^nZ~13S(L z;}v@Rnb-#4dwSMpr@AOPUI0A$*QG`%^K0E7TU+h~1K--G-ctY?irftEVk%iCgm7_R zk)-Z@`?bY4059GA>ZY%Wtw;l?sn&FH!is|Qhm3;Vy=!PEa$Sy6Ovjzg_~xK8FPJv- z^K(;bL6%lh1}dl+1-gA}x|+~eMUv5hyEP5f##cGW_dL`_Ku&g_@Sv!Wa(S+cG8tCl z;FjD@Gf^$eWaN%dJXDt|L+p6}03%FTqbG47W3^mvfS%q}JaRt@mNpx{Y>}QSfO#W) zM}j@YO1LK{IV0AzxDULOV}9)8*qTUPCI0|eaz|m>n?AwG&T&!4BBVvP08hOu4&{j5 z3ov7X4;eH|CRE@8NbgZVBAvc>2M3BDWK2MO{Y_6RA2J&5k-pY&Sabl>UQ{@l%M-815jI#!WKn0U`$gle>;8aso0&%y|Q~RhceH3==R06>xC8f-9%^ zRd8aC=@_5CkzB6z4orh+Cj)hFcv|)=HJF=)1gXFT3W-yc^s$ucBct&HTk1Bi8yWuGbhxywLr{4w-~-EufrZCP%+p!z<2w|OLtf1X!S4-+G0hZm zs6R6T38H_6{u7Aow6#;uIZ=xH8gtWA13Wq{mV3IWu zSj6hNwkQWCxG{C3?AhIeSE`aYp9)A@!jPhXJu9(=g@%tBlY_UldG~=eYiWEn0cnOy zWQ_C`*J_&8h_OiHyh7VY0kUiJOnnK?yq_!9=jwTNNU9M{^fa{HO`1$IaDC}~Np2g; zze0K8tCX|Xt*q_=d$IJ*Z_VO;8vg)CFv+P#u15g3O7mV8s@g2|J*_6UG;AeF?E!I< zidii!XN;j41#1G&#I{jDZOzgW4;VFS)5N-9=4;eYJ2O*_3g61b?P>Q$V!XOGwjkPm z@aM4=p{RnFX?+;`!{)_O@h-2T-MU=c-$l7c-rY$jh%}kBuO>_65~&^QU#i!CVUsG< zznL3)ZO@#hCgAlIJ4A)|9=`R@>AoV;%#oG7k0*>4?^-f=uSBDLKAV2l&A>e~-mm`v!e!z9jd>e;hODh)#2P%;iFavy zpD4ouMQscwDgB4{6D&m^=0&d(Y0q~W$cJehWOS>xQ9O32jB>zr&1WAK=+6+2YuJy_ z=8?Q#qQ`^ww7~2Et$wMC{zvy2yhRV@MwF1^PCpt$<&M36m6F~!(Mw?5oS$k#<6R9t zX>PIaifajr{zvx`>(KsWYB7r=4c?}WlmG}G+}0QTCSD%KQu93Z#RJCLBAkn%j=A)$ z77rHw!~2Bs6c^e??ZoVsl3*wVbH!r#bjS9lD&P&r)}p!b-ib6199N;TF4$B*I`j2Nj6oPT$U zhUdq+D3frI`-*>1#^2z7aK5>Q{$=-@yNa$3IIPI0RMgys11;96zmGJXga#+lvgGlG zku>cx+>wLaR*}o`zw_<=#5`sf`O(p8Zm?V=$_dMJ#aPs25?hB}war|3(?+< zu4H9D?DV299q7=+Q|H>=f~)&89sG~(7uRs9{A=$z9V%Vca%C`bc&hU=m{@a>=~+5| zk2Gj^tGj&1(xQJI=^|DHum|$0S!Oq_H}?Rw zT+E*lwB%ADgzofWY8ZTdmo8&bpHo^auN(Y?{d)@j(mK|4B$C{N=~+7237J#1WL*3~ zRt7YH1MF(O&yCW1e<|$;9rISQ%v$>-yj>6GHguW1$go=+)tj4zf+oS|9V?bk7DERD z5{z#!dCy9*f8vj|p+jy#$75Nh{O5MVk^SLa zwD5J8Lm5_s>Pl^w%Ny4x9M(>*Nna!H{uP;hdd$LBVb?X?OAS#R73Q0| zK7fnj*3pHt0e{*w+mDJ$=jDig2E6=DBQ6UJcc~+kk^v*~u2;iZQu_seZ)2ZRyoch1 zVB$zrpFvKS;&L`Xwjfsp=JE=jXu&sHmyMNxsruDv zyi`za5d-x#%$dR*Z$6ptLphT>Ujx>uWSBLy2mP;%d&uYyig?b!lh%>ET_kIW;{^NH zJX|gmw(NJsJI#k1LY$twQ+XyI`3L>2iT-n7_^L*5Uf6TrG_B$?a0Dtd*NVkb5r-Ka z$?Z=P5^w_J+J%zPzrsK5d`I(}BVH<>ENVFa06poWUcm=){#BEie!jGWdyWbG>XuDG zeUu;C)%r=BU+|E@C(73O=dLO#HK_^Ok$iyk0<%yR9R9S)p9dk2&{O`$!}dY_t%&|+ zY$u2eWMbSd2U2NHx=Hz!PI_=_6>gXTxeuVFYlm_&2=wBT%5eRVe`{htnVPa{%X9Z0IgDd9ODe z?s41-p(N|U+xZGXF_7U;0=14+hW=D9?Ho_@KfJ!$l=6P>aSPdLlF4l*9z+M8gjHWL z`ya-a&XGm{{VKn+aO-HG{?WvDEE(ZXks&|`=ZYh|Xoz?bR|l@#RW`WXb`y;EH2(lK zcWe!uQT?5V{!(6JiT+~Tnw`{tGERNQX@gw3GJMk#2d!0ha5o+WAaKiqKT2yY!~Xy& zo6K1ETpDhh7#>^@+kM}zI@UIq@khbB9h|Z1 zUO8*|U--%qkD#ofL8o-Ac~!9!o7J6{gZ>_99uKuPw-;woB;}rPIsB?06x!&{Lg{)m zHmpALMV@~;g^ro<0Ls+ys}4}M*LNu;wHT&mkgWZR>8+b9@VwuUyZ~5 z5a}&+i1tY^+Fc3#Ysa*?B)-0T`3dimdzx{i@@D*+~(Y_`!%*iqW74NCD> z{c|6wsALhk{J9wGnu*5#nHco0sNvylj?b*rx5P{}DS;>3udN^2QWp7_Bh=M@D<^Xv zJqe|f)OMKQ)vPr>u|oRIPv&I*0NU3mcZ0{SX&+a*kQQhBGgZVUHNkL3b4srX;H7A= zwEqB=3+oksnU9wKc>n~D!jtV+EH=5vr7!MV8&vhBO^oCg=~_n(Px(;2y-wmINEkZG z!1Wk3`6sw{-qSN2b!N_Je=K%28#?{aNc7EUs8e5L9eK657MXccewn4qI(+WH`evJ! z?!)jj802Q_fsXXm7rEw}j?^Gj=%YF7S^EUd)*;vy;hP1IT4CUlG3`@s0A!|q zwHroUmQ&xQQlS(}uE^NQ&^O^xG;B|5m5?dsnBhxcjMXYA&DC$A8ZlMDQgP`|m0Yg< zxX&jv@Qa*uq>zv}!4-bVBPTnMI6E*Z0L%|3y18_ z^R^~gxf%DSFnsVjQ`7+2QP;0D^Ah86{AoDYIoVy+$lSx|aY7Tb-!(J9z+-l4s|g1s zt2svKr8YFk$~xza^rne2GIDBfAs?^hOc5&ly$7{(%9?!*RcG8`Z1M>oS_1%gH9VqY zAk=ap$IFvhx)C*1m`IT@3>atYQ6wnG8~*@Wej6CuQIbYJY85D;b(r%EOUUc*QA-eY zIZoYbMJ{@_MMXCF^T`6ABw{C3vV_*kbBttq)p;#Lf{rTH#K7c}Ra)mA2LiNO8Olwa z&j;n(igJ!{NjafGQ;=y-*0pfCz^0JwZEuxHqbv_29>${&x1l-pqDJ1mezd)rT;jV0 zJw|=$3%LUf0QRWDsXsPe zRM2}yF^OUF(GyA!-sYqzsGj-hLa983JW=e?UeVYYrOyP?rPC&O6dP%sE9F_o&ug#}3^315Wus!5^J3W#lhx`-o+gj(Stp zU88h^r>Ue017VE;2O+We8h*>jUfK5_9FfX5A>bN&MnV@1e+puV<4iEdXj#-Q)qV$I zPuUGsxGEMo9P?5DBRjB19jXUt&&*U3JLZKYzF-H_GNUmokrMx zP6bBts*srKdBq;gJ*9`UD3BL9&uW`kR(La7g~OSi`823B4!qH7sO5 z&SU_3QhD)~$j*K0)fldZFHlQuO;?>zf$O%LE}wlM3;nYBKEkK-Lk?Gty((3fKa{%k z6#a}XuvpD)BT-+Q`(k5`+zP*MqFG*P#tlnSxq*~rWP}3G^~l9ZG|;fhui;IVWRVo7 z9>mggV)_juF|>)-7Nc3ZXo1ew0B7*4@k157Dvl49MQ@qIV23=V;SQnq>zOeC-JD*Z5uq! z^qmd}-RaF7qPrzsjf@;|){ish`MJkKQR8XO57MNLOJyXt~1E!IHycfVj4WC?hP=-@^0J?Iir;^r(#DcK-|1`G{r!zm0bH`n(_}U4CmL{ zn+$|>JXCU(%t&MaWuwPD{Iu7JTMPi_y*bzt10KGV)($@AIO&l~lxlY)Wpq3a-n8J$ z(2<`*M87b@k)HUdq9e^Em!%w_+8-v?IUI4$DmQE#hUron0qV4ivA81x(w``~h@>sl zAIgQu;~hGRQcxTPKEN0{A<3Y03~0V?Oy6o%Qm zH`a?-YD5?+{9ps`P9zbuocq&_p(g;TG#fW3860#pnz;Ff;A1)bDW5Mpj(s?(4ZM-A zNF;jG2_TRKE>u2YnIn~uanm&$NtPt3KT}irV6F<|K9w1RYU4khXUaZdiPW8^r##bu z{^`XbM%%Xo2dy-z&kTPWIYXFxMvS;1=hRdib~Y+Okld1Jd`L399{g0wF2hmE!qN0E9Se;Q*I-H;E~ zpwREi`D^nwbL~+%$;mhsWW?FXT4TWMes9vf6$W{H!~n!w2o6WJNWHoeDoAZWJdFEP zh9p(a!St(87cm5i+d&zmjUS(sesy34E6Ak%*au?s+a{kV)U_j3>IkD|(gRPHIcZQpd_n#kD6U~@>sE&yzE&S`Rkkt4e>z&s9vG|k~~c4YRY z`AYn`&w6hle9PCRQh_bpRGh2jhDWAon{4C~nFFRpAc8~kvA{l*=L)|oZsc$US15#L z+BV0|X^h|yqCR`poO%SQ5RzQ&#!lX~TtTMpW3p3@rjAeu+s{yn4;139(2>P8Hz~!r z+L`_#%^`w!lP5UOtv+1>N|41FC7Awn-yxDrl?R@Nq&E2=Fdu~+pfsx!#yzu1vBw3m^fa=G3+x!e z^d^=B2oB&m=K#}D0Sr<|>H!#DSQ*|2d)&kGEu0UotUSOg5~!nyiW zh++pBqj*%@NTr5A7_%RxFuD;T&t5Z1@eGAioO;rk!35@!aUtHagX#qq0fkCq%;Vmd zV=(0YRIM0bcVzo!n;;=h!+o9<>hS1h(Uz)S9>vl$~>i!9Lw;MTp>MijXqMKr8g-n$fEb z+59t0lm~2#a9^if)QgOdrB_)UMo%F7RMAAu&piJCdYV=NBt%2I6v4FisT<{Qm_R|(TxaV+Bx=9zgYw6{fScc{~%0(}J$t73P&fo;@kq3_}&V^S!3vgGEaA}EvPP&n&R6jF2M9y?-&V0gtG zqGU2m0|EgCpA6D3&dxcY#3VUpAC*0{(j)V?{J73DRE2RCSs&-!k=mHVmLLzUG`gw) zxyRO-mMzeM+|p{`JRnxw!18(nP7FeymmGA>H3vB)pG;9QKFmgk@f32EbT}_Uzyax+ zSCKbt!TN=&@w@%$CIEUGnPlE`7|1^Ka)&Y$lsxlHXW!iYD0X$^$AQ|L<(zI~jGj5C z$~&P}d>@ny`_yr(hbB*$pMI4fK%*f!q-A7n!ZC_DMXzzhw%mj{`qNz&M9WEoPig*5 zp$FEZ-bu(_l`cAt=?IYp19u=E^g72Q2583=ZkhR9{uG-TmCGt~QSHd&QRWprPEAtL27;=2!vK+kNYJT2GG`R=aM28d`O_`tE=f4fIW))2 zZ+J6Fxz#T=$vQ`FqxXf|1P^M^y|nPgfqx>Sn}T@&fJSSVV=l}ISPx^0Xe^j!B;@xs zJfgcTUCkVqGD{LFhHexN1uzC9C71B07PgxLBm*63QW%sR6Ow&u(qVQ;^2U)OmIQ%J z1%NW-DEFj9R(x&gPL<(t<*?v(p{o>_fD$rEKJ>{RD9_BNKU#eA#3RQ3gP^L{1WH2| zJanWNNP9~e$;JgWW>g@7(9=~^jtQwFP|4F2^cON4JcI9+#(U6{W-tIe(Ji|a`HvLX z)uB8u0An=liXSG_BQ448Ofr?>7$0hxA8-r?bJ~(gB~TQ?27N^hBm_Lkl#pP0gHwnR zkaBxdQZ`}^I`*fmQX_0}$?45VSWhK(;Pl5@ja+Awz&zB&c8u;Ebri{27#3ChDv|-; zAp>ckrgQTFRpPglOO}xI=~H=Z8$M@F)Sb`^hf{`Z`cTS_xbp=m1(aha=}bu9apdGs zfMiS;cg%R{igPh=9RMJ8=A`>HVM{sZ90A^|M6yQ2w0vTX0FqV=LZWr=P4e@Tj)Q|z zZy`5eQg2xX+}L6@ZGeN0F;Gmoeagh1`KHqV+sFa>9&&i8 z#Lj?*#(g_fATO`!M3J+fmZrc;u9FondDk-CMIYkA1>PY5rz&C2JL~R*jy{R;i8RACC9$r5Rkq+*or?pH% zu1X4x*fQZ`#Vx=Lq~%>HjHDcO8I6IUQ-HGyol^9=NI5 z2_z9sc-ck-WP|BNkQI%zWw<#M$B2gc25Qofi-M=EHPa2b7#!5NfmP!QaqHI<$)g1~ zEm)8^Il!nU+(966J5+L)69ZXvF3Q9XI^wy@=K+XTJXcAs##FNb_4lq~`vuC5wS=OL z;_O!wXXPiYJ~^2??gAxPN~Aa%`3 z%7kDKt$N(2m?zIF?%R)Q1Wq{wQbxo!)l<((u+d}R+N+izK$1v}^FQfNGvwhE9Q#rj z(d68~;B*45Y4(El?kG^~Lf|{MVV-ETK&Z(pbC3t7I?{Ore-N$T9Z92GG&WuzF|kz! zS6=3@{I@s@{OMT*#M(2-&OPXm3#>O&f(nlhK{rgk00zV@;&IUVD0fCHh z?@E!#PWF@YrvQYIa4|~2Gb*c)In6b)$K`H2C5N%Ka$R8c`B%uAdPdLl0;O~HMISXD(| zk#I=wPjDFM6opftQ;w9!-MbrxJ*n=F7iy46^*E+5c$A%hk7G&t0C{BK_V%Ha@TvxT z^HRroXZAlRLjG^9CQx*boOPrHY2j}(4<<9&O+Gh{E*B?`rk>Dwt(h~=QYowuIobvX zOw}ZSMK3l(bjLhX*4doy^Ed*fg_B?mqd286kS8+W8SR;$GM7}x1}n=%nXXXx>RpF`JyQK63 zaF{2WR=5?JBajAsDI$Stz1pP~!6e-w&N>1*Q9jbZCm)R#Ktv-O4;bf)ZsM)9f=8&S z5_J+AcRc+loG+J^k%!WYU4aaDFEatOZBxiJlVG;tk`Jd!me~{`VuF-IDv%X8&p;`a z9f)n6jJOBo`clnqX(#N)bk5O8G~e>YDr<5XvrfO z>&;SE0Yr-#+`w`dQBr-cL81fD}Q>12f*|5}a8xNTnf+{wiP8C?^I6J9W2QgL3 zG5Y41QZzf`Cz_Gsh{w7|ImaXlt8FBJm|UN1(?Cz>D~tsL-lLINuH`-Y)U9S(FvR52 zgxH}EB7h-H%rTFxJyJ$sQGgxuPH2jp;{uRJo_xX%I|=|Lg~=>feiaKjh=(h{>&00S z$YaU>0KTdj2rLkeolQGj0rd-Jz{nq3lWHi)JBMr@l^C_PljkVhbu{hk8(}yfN-WR` z=d5`EwtWo-Xtam876x11G@aKG&0tNolS60P}CwSZ9#!>(2$$~^@`D>N)O3b2o_VV`khBsIeR4BZ-HtK|u6aku*)!b@ti^bj@TL1RD&(~|%7E^7 zKT1}(G9bXf_pbGG&rP;5Lk0>Lf&5sh@JPrtU8HT;AB{O~!hos?KD9*-kcj!~?TUT9 z#l{Mt3JK(5qB0V9FwH5tk%rWd6agzPPUZtNj_9KpIX&t%X#;Y+A6jgRM&C)RP(~nwcfzh)hjn|ACe6kDy7&)iM6xS<)nB$Hrv;sKd{pRLkN%b_c zU=6H#_NO#bK;auXsf1$#deQ=#fU^9cpTebZnF;|U`&8LQA0Ya1O<}pWlg%`)0xU}F zhBU^vSjGq*#;ioUn0%uY`F>-C#}!EdpCBrC9@R3DAYG}BImoF?Voq1;DJ~=c0)QV} zQ?dd!k&uAuIH!pO(EPaT=}iPlg)7(AohkBDBLHKZRJ#DfZ@7hZ;-z#bfkEgh814+9 zZ7c`4sU?s+ctg)}X=nqj3I-xZJerzg9@43voYgqd!6A=5xv3gCQMWVTb`-!>Qim$F zG>w?f*TMD8MuCsh6pX;RkG46ck^uzFvF$Q}?nY@PLhKn|C%FQV)p3^SD#9YXCDWi3 zz^t*zPVA0p;ytGfxut=OuY%YesnSLxa!X|TPy~Ctyq8=kr={-rNkk;}0Ml9cD9g<` z$O-pHrbSD=^cqA8rY0`S?be*~$^a^Z9e5PknngnUhtPJW+dP*m^5iP=d8F=Eg|vAJ za)YHrqDCJyojQt^47%V9W7d)5#G6#-){C(;G;hIGlOBej8FpQ{$?rjpaxgp7!9;&F zf#_I4i{b9E|m#0hFM^0gt63 z@D4~I`qTW$GmPh+^&G^;yPFiPaMX6yA2!~7Xafg1q(BM+!>>wm`Al$00+Ut&C4`Z< zvB#h|r(1~nF#2YkBaQp#6s7{FA&1hYz*V}D7GK?{9=NE-n$5XbeJbmDBR*gmHCTlY zxp9#~0goz%!>`O~!P6VCa(a7Iu}+eZvPFz~ik3*18>1XkuocmkPn3*$WK$%J=KvnO z;-@OCHt-LrrfqOCNaNC-kP)E>2m|$~q(UU~QHbY}nw79*-bYGVR4~Zmj!+RMnK<3~ z(`EBm?%?CnkgTi0IUh=DNAqvVAB|0r4U$6Zx69_JY8qj=iI>;4XcEXiWu*%2GNgVY zrb!Ju945En)`V^Yn&&R;q=k-D;=M~zv=a@oqMxO5+U|#!ZLoklU{^e%jTDbX3I<;t zDWxZwu#L2L;)*M%C4m(li5`I)M*JDv+j%Dw;_F zF7m6h6u^LjsGEtRijg;AHath>P(K<;1F+o9k)8meiUv9uOAdZon-VN#R!n4IQAGeU z-q~TxyOur6F*NDi5D21*wggKcFSkERka=jNlaoahl2-w3)-uE^_3fG+$~lc!CWfMMMK;Kh#1D@P{4{+fz*~`jGoj{ zPRI!oD3^l#j+G#fdS#n9?L`#QNCqh?c8)#GGISfjQ`GgMieOBu89=R$JDO@ryNaW6 z^rDIYFr~_akOx{uLAW_Q;)*B%1E9z#I@Dfk$ix`N-1jt5O29b7=YtY~mgAaHYSt)n z00*fQQA`EcF#z8p2=}tt7L60mCdMKcK0>qKbBq_-6 zQV5Hak&KRLqLbL0vJ-C6Wd~g6H7rq$xxvqRD5<*@#Xl=;3rd#z{%G)hMrfj(cLCl> z3P=MZr8j#i8Bk3WRRGI~@!c@8vqFkPw<*3pqAQXmhgb2Dfi^kGk|uM z=M+&zkkh#wp~wJp=xBi%PF+FwqKaIoV!Ng?7pHMdGsrkhjC#>UFc{1wZ0mB=H|9Z^-8aCE{Jq|WD7WNaICr|!o!^OvYf{Ty)qj5?9qHItVg499xZ+x?f-ZlfK7@+_L5in$#ad*xJ<6(d_jpn@tEJ& z^iXL2L9p@beFH;t3rj0& z8(TZ~FCLy=-aft|p<&?>kx|h}$tkI6>EAOl^9u@#ic3n%%4_TD8ycIMTUvYj`Uha} z!J*;Fsp*;7x%q{~jm@p?o!z~^`v=I=v-69~E7bMPf4DFJSpN&_vHyR>Mf!*f^S@_+ z`yVb0Os~fcixeB@CGQh5X${=ZuFskHg7C=SC;qJI!Dr^zL{ONy{ULb4BCx@V{14jy zAp3s@EcpKyvi}R%|BY)AK!k#{p9RM8xlb)3u91L7n&-Gt z(>?3H8Wc<{a5VOaMmA61q!dfoicqh>J3f@LHHel?@AxuAfnrsf(vJOJ^A5fju5Oa! zHCTPsWWN2DsC_QpUNp3Z#fdH$k2yDcE*~N>vpp>k>a*l0_ajj08r?@iRXiz~{$@bU zUBlsmi}Ry4Y_|JA)t~(VfZdNoBA~}H1*yq)MvHIV^fCH)ptGH7Q-T0WFLBSRI8IpW zT)6X`A!4;cFz1~4O7ATl-`u!ZRf1==@f&|i)i$HM(YwM2z(SAfS!( z;J*DLv9{%WoQ#yYOhAe!qm7YoN|e*q#r7R}lPIvV-Eb*(fX~HM5w|MSlg`AeG8APo zDblet{SSap!U4l}d-u&a&o%j=AW7M^;olPcnLph(*Fr$VEIbZPqNz77*gr-I-`=PG z-8gNs|9!6O7&O8G#v=00(z0CIFy?#nZvAs8n)S!6IUx@L(5rYzA zG5IO+rOM+E`YnsWeJw zLzxq-*``r_zHKXYK@z*$Qr<_Z$#7OP7xBAx`Av#TiqD#77j*EToomDN8A@$m-~sT= zmdpC}Z4K`pox8DadCgME66@FD(rg&6;TcJ2Coj?~rW&k^{8BkbCaEWK3HqIRne%Rx zJrg7%O|`e$srCFe^ER(~rwL|hpJOoOIDc)uX&TzDSaA4nbDYTVciOJ%z@NH?x~WF{ zdX)Sv*nX8JNrE5#qSm-K#5O~Y$b9;2)E~=ZgA4O?R@p~ju)UVpoO1wjYmNx#l84>D zrup(w+|VOg?Nd(U*SEfUMemE=SctGB#KZqU69;BOvrJ0b;u#Xn>iGDY?f30OufYF@S!$u+cJE zQpI{(cpTeTZ_*U^0pJkF-angYyDB(ZxUg7Dlq%68dPas~12SE52xyknDB|X+29;fO~&mA~Qs-L?0 z8)n&7u_R=SH7k=BjfNUcSm)U;3FJ7P%(^h!oMh*~;~N$hPzg$;Z|nmyy4Pv8Jx1B6 z(FU$%gafH1ciYh=uL>_IW^85=gaXblJyw$Q9MtI`BagFR0ZGgP+Z~7480sznlp2?XYBcTRgY8uC-!|E z!5on#)eN{aweZqnUFq5@U=0YNRP+Fdl|mdX45qEZoSFh-M+sdcYum{vak>Pq6B+g%aK3dnZ_lGjde3T%PJyBzpiY+ZRVe~+tl z1PQ+wk1&2=bW`)Uzr4MqLTZ}ckKEI)uvvgvNycxPT5Pp7VeK8QA9N6=_t8p*t zo*FGWagXW!Ts!IcCTAXx_GuyGHJB=UtBDG2-}F>Yr~FlQ$ylio0*K;VLsK@v*XmUk z)4TH)vb5SJ#W|9$X~Of963&k-^z6pcCrX)UpIsFHa^sKwI9$M}smRH@h-)Y! z-GOa3zg6|h3X8g!Ql!AsH$GJ%an(v~wZUr42EEQor7>IP+~*=vR==w#h`0Xzv%y!Z zbMnHRk-cak>q!Cok@x*+ZXzrQ&cA^uwmJ0kf_vJ8O&M=jw4#Ttr7aWw;w*3Tc3wIQ z>4YxNQPnM{%&nUD2@vP6?v%VeF|4C4iESy^73O;`c@h`?AktP%u~E90 z$}6gW(qB{({Jdfoh7JK2*)I!FS;z-~slPfbmopP=ygguHx}$Oa&jjsCz`GNLoQFUu z<0_6hh02gYL2y@@f-sDT2Y;qJ;6IEC4`QgfS24NRY5tHEAogY_Af2y4w!gQm0} z73cF;FvXI$ALl_;cHT=ACMXL39iN9n3VTt`yutMQ3 z|Gz{P-rOvAl(s%@;rKFAQ<~q22s*X;pp*p;vwQmSED0toc+JkxY{$Gb?@XACeHFK9>VUNmZ z4LLk$)|btqvdemVRT()qLs$-?q_=XhDZX9lDz1x@I7fsi;;v}aJ+teW*2P>@s4ox4 zuo5d0${LpBBKgBz8?B7UrPqQbk9}*eRb=D{QX|;?}{)}mFdSPPQ>D&6au_O z2SGJMlXMlvKE|#bsGN@6ac;85=LBxNnd7#Miq~D$BYNgbEUG&f9zZC~I=rMmFZg?O z^6ihz;|p~lgd8DWtUQujx$^Rwb;^q4M}m&v@54u*9qXZxr-}%SsQTIXIdUgyiR^3O zB4%S!c9W-Ii5`i(lJ<3;d(&h9o_v8J7d)y#%#maVn+t8g;xA)XYA-2*0He*kz%a1Q_5K&5X{o{7tBa<=Gn?Zgb~WBkRSD9VS>Hc-BP)#RBN*}agM zH(VPq%wTOPY}K3!ZF&A8THtlT{$16tsv}8AggsY*}GK2{6WMh+rX3^+(McNcXqi#P_ zqDaw}Trh0pmEU}H_IS;9rhJc06YD^~oZjGS8MX5^=N~lf?K|%X6Vk45v}A-AQl;lD zi1gz@U?WeEI9H5wSQjySV>9sd>P;D+Q0LTSa~^rX#2QeNnJhgsR2T8Kx2UQsN$RF- z)Z8$1hjz@`it^nhzcOybb-i!wJw*$GBB_${puc&7bYkNiUG;5HAI< z@T=iUNAAL%nF2rtq&b>g#vyeM24Q9D{q`ws$*= zy}u9Rb0407C}5v?sqEr4{$yuAgXo}S5r;C!d47|>i*e1KQv7w|>)Dw{lC%)ZM_!k} ztWfC$si?mE$40VYl8n%kPKKG*#FuS=NTulq?!#V&|Dm#|jStmJ^k%+vkLdSo(p_c9 zscf)F=S45Qb)Rhh99$5u>AK1dH_?IXqpMwuj7`KC`hEI3)!9ULpGyWWg|=u~oP0v{n{72E#0uJ?jk|eA6{(g{mG*a@OY)Jb zUW^~rsjICX09%hC=CRXGP4J|TW$4DRBnI?Hi7s7h-Kl7^VYSJD1gjo_$W5$vBLCO7 z{b$`!F6a0^%G6+5IZZu|-)^Aq<&=4}7k4{c?DGWZLdL`EnbsOPLR|gJB`=FUL9nsB zdX#iaivsd)cL0JcMSn(03@r)(Hqm|}PQv9Ksp8bA`=Ck=a$F&0SPnSFlWGm!!vu{G1HUCaXNqdox=ojwPUP~Dcw850&TlU4CGj9r^uYMY*tv!O#|kTuYOp=<&WrkK7Y>R9KW!4q0 z@!P%omK;x&xmlW>I1|yQ5!)!izu}(+`GAx-v+Gr^n!?D~=83h~+hw27g&sHDs@nsf zHLOKE05G{uGP#8M+^heZOQ^l&!=)+*)&`W>TDn)-3z6NQP+Ey~hIe|-io3GMgX~H= z_)b1_Ej(Kf=G^5aw-(o4`;gE$y#J@RUE|_zlD8ouwlq#%Z5}$C@JJQn9snE6ce#~~ zLoJ_;vpwO!$*xy>Q`Jwhs5soWxp~)iC~l-51&@rGdpJVW|5-NG-g3d52wVGaK{eZY z^Zu0mI}<+R@n06x%LR>RVVd^WqYr?PFmaD^@2$(u5Lrsg$^PdwC%5h9H*@ElJ{9Z> z*0JyKqkg#`{m;nYiG4QUQs<776P6s8rKg$S_NRc<^A@Gfm%wL}3fGi)Fn+>@OlqZS-X; z;Q_RHRUd`u)^qJ%S9?MFlkD1?=uU8do>zzFgc|dI^|wQnDAKqHmj$HlO`Y7Lu6s`lH2pX{~#k_-1YrLs%)M!xcHFBL&Sb zkoRN;vxyK%?^hPSbSm;yH${ODGMG~PlPO+MK!r{m4-2v=gs@zs9J}hd|Sd8Y(C0a(H!Iq(aiyefQhI3yY zLrb~jfmO!cS30^Z@_oER!uLA)DjQYf0+S%ufafU6%Qtrkk_^>N#ZN(M?`B;ycG_B( zPx+u_dm^I{=UdwcK#xJYG*a&5U$-faqn7GqYBaslsxArRwuRo%8g-YADJA;TOx`g~ zXyc-aMWkb(#*W!2 z%NeCvK;axu>0#FRLO#~-&wW^vrE7MSLcga}UoZOtm8vLKiYlDb9GY6HwgeK?a#)`x z?)g0x2-0Jk+RL^QVE@a}Z^F_(R1;+6`Mp_=s$e$JQHw^9^s?M~{^jpsl4J5FV-iH$nVKjV3Jzj=@82xQt=qLJ$m*z#;x zT9+$P1xwX8Fo}fRS$+75Kn(G^ei! znJvTpIZIJq9-a_&o0A6shY}ve&eo?E5_1e>!GUn0SjZCq>443ms6xT^O=f>h^K+^A z-+X$L)l2*(1oK-p(Dc&GjLUjvY8kC`R3fqqmWw^}=r>*<(()Wz70x=9Kfz1PjfgJS zo{M|E_6LJoG+v5?D!n(Kz%uEmKza4v6g&WWWj5&z#k9ke+P-#Luag+=T?EF0aL}(- z)sk&m(J?S9%y+uLBF~SzjX}a21zb8~TJk8a)=kDsKIPYNNex&)bM@ui`bu$_RZ{?C zE+L`O)5!>3_%y-hh591@rd55C0g6Y3#=k8V@3$R_0WSD^WaA$LD&H&1cy`Z9{Mf)i zekXI)R`4Hxd6@69VVppM)->850qj0Kj`YFZdH+mp6#brQS*|V?7Mo{a{W>n}OT% zJ(W`Ja%fbQ<@}j3xF1N-?&^LSXKtK>b=km0mVJvAou!Tfd6cTLwc3`jR~zKS`NrOK zb@SjBaCdFmm$5}Dl7hRT)Dq#oiT?~JK@I9*U%tn61qaa0k8MSs)24|MyiiZpyJ~qA*T>?{`a3Y9Jr4Nk}n-0C5>&(-gpu^vtkyBKhrbqOpzD zg-bUVcL=PvhMQ92*488GM`Ou=7q7gCe63na5MD9u${u_6t(CQD4(Z(Trf`aJ!%&ex zALg#738Suj8hsi&CJ_y*CTP(RSX2R8bdDQ(oX;_3WCSaB|I0hz7Z?c*>-pu;O=ul5 z-f6!}5_V6lPPmr(oNV&dfxF3Bu?awzkZvw9Va6fT29Qb-TBmL$o;30QDT^0 zH_ME3oY(uqK0bXpMeE4ir(P2kuXPmb&KwrAeQ0ZbqnYpWi*1(F>N{Ud4eouxXtp~C z5kjPYA)2C((aLu}EPM)q_OV|*w=bb_^&_f=fuyoUe+S~0=foVhWWue-0Khn3SmNe5 zg}0dYqg<}Hsg}x`g9bQ7@4TIt7#bS&r|3v=rfflI_D)vmPD+e#swET{qd)yZj;1)^ z92t<|V1Z11&*=$_h19<(f2NV&eYL6w$b(k-$cI4m{E_S*C(mtuIaU_%>@l4o)hUb*i5Z#w3O?BaR^+VuZDF?S>D`sx^R6vZ|3p#;vjGpfsj96t zQ7y>lTESrO1AuItMGLFL4Nq{k%&HDmn3$YEpb@vF<{l6+p^4(}xrE+lA;r{i=#t8~ z-H&JM9Uve?@l%|3nNm~^d&ubP)XziXPWIbYZQMI6%egH>qhB(GqS)1m`tl`)c;!Lg zbxr}>4}hr38Y*#9t`+Cz{cdxkh215nFol>T5Zd4c zB)g~eX4>iwF+en=yycVKEtTSrwXT8;Y?4H^s&zTr7MPS$f2o_#+J>j$!#G2c{#PGe0=xHl;Wi#-OdQDUmKw{GshShNnOqf zDP94)j;N_|oV56tEFXH-fq%`LnUVUr>VF#~8(V%gW?^m$4l6C!QbF?z_lR@9b7@FD z7cfuB`PYUeaWI=BUZCFumnM7*pRAcZ<5e7_8W-58mbb2W6Yuh3C=!(OMM+3}uv6DV z@)1Xl#f{)FU&@yUQbigBI20DU+ET5?3dwd2f%1=qzsMUT_i@C}}pyEcI;8h_Je! zaISDzOufKv5_yI(cnN(}N!QcwnQws+7hICZ=!URg%D$Ve&Xj>Y5=#n*U$e2@oQgoV0G{l-qj#lJX*e`vY1(Vcx>N&R=iD+yHr*a5SF_3-GxM_Lr;+cn@! z2K&}AeZuuK7D;Crh1o(9xkdre+8QS_^)7hKb~UBXoz*9+a;6EheB<+TH_~2=9(ipo zG8&MPwsVl;le^cWv!njr{uWK&7A3rwIoi_VIYx4*$ThZ)|D;0cIJaG|^QU?0iO9Aa zKe{Y-J6_vw(n-v<^}Eq|l_n{RmVje6kxp33SJt>7>@)55ZuzHb@oojX4mW0buV`FX zVDh8ce=dKhy(sT5<2xY zmx%ZMP;?xV*LL(bMl9@)SnV+39)M;Pz4w!hoivAN7aew2_Dn$FcZ~J3)d-iqLt{`FG<$O z3NBs~{u?c-CieuTih4*BJ)M)7nVcmvO(P%FJDZd9p=N{W8=?uR;O*Df z>HBKnNOf~HpZezeyI~T?8KpDBKU}pmTkLcWdjc3fn$*HrMVNwg&REFmfIZj*i~zjk zgj+Ccx!P8G@J7e-rQfTAS6mLBLBxcw_jUF<8@>YhJg|K;w~rqHO|+5Q-~AN_R0QZv zF5f$|OSGLy9gTvalncFhu_vDogaxBYQMosJJF@fqOC5%$gadd>{LbSWy2tI*r7Ikr zc}41y`{a&M$}Z{+%1OWIDuWNa)>`8ItO{dPnyrRpuJ!)1_dEblOeHarq7Y2eXj9^g zD6Wfi)sbR57FY?AsilMV9`EPvr{a7wWJ&msnuY|i>dIlU7M&8ql}z5$h=h2CR!4Tt0|R+935bWt|! zs&BkLE$vavY$7^P+Z0~*-l9nApGvM3jMa1BepSE{)Fjz3>HSV9L^s${`0gMiugYw3 zpgFucq%ZNPId0q*$JK0Q(-%cy%kTgwS*|nam#30Om)|w4{!7iq+CS76_2_*7SoJC; zeuoF7n$aQ(i8CZ*^hT%k5--l_e^{V35%sxT(^X_*C9ya?vTl5KAY$~B_Q;r+D%OHf z_sgicotuk_Ty|7eH5wGghPcmi_Ik5@LaFg@>0F{Bb_QaJw1<&&?inRb3OxYQbf+jj zO7KRnrf98XiPo7CsjKA4B1l4+$m~sC)wtk}(r1SR?ih&X!SljafzCXAhEGQ?x~YK9 z4Q6@YDfW21@st^}^b?&}oZBrHRc#Ht{S?FcUBOB69~Kg3Ta$v%xZe)UqIUD+jMXnA zP&t*7t#UT-N>1OrTT$X~6uuVp4P=0jOjw*93vj7jx=xxlQoavd*}u`&3#?a< zvXr3=Yo|9S_wKEql@gj`s`a)@nspOp&Yp;93;zktXesvQfqR`o7a;hq*2EZ;{9zmt z2x5DU06MVYNeeP>AcqDon0~e1~5~b6`y1BXp0e3j*@>RCnm~7` zWhB`865u}9;gWf#$o7-m;%+NevxM+NJFGRu(7FP`-x1x{tAX*&iY0Ne7HwVj?x!DX zxt z!xTg@SQ1|Lh%>m+{-`TnlI}4p4ez6Ij=#XR_gB`fQ)lxg|MZRz17vtDq>ZqgZQ8`E z--&$g!9%Q0fPa{IYrj!)G`CJ4xWN{)(M1LM)LeJV!F&=keC-;K%dm1d-Ctf8%ldRS z0;Z@@WSZ$LGaNwU?W~V_?dV!^i06de-s4KP7oUBj$5RTCo~h?D)dZ_@ZBsf(d%jm3 z^>^NK1YOdjBuW-+14#pT4!x@QutVWCNfE-`V-uhu=sTp)G4Cz0x3hsiPj3K)XkCBC z8P6B=lXDU3Y7TuBOQq~FA<5!A(w!K*(qY}(gp_}1&gz4m-T0LP?wzk5qpC${7Nk|- z2iN^3ce3S51s{qFP=0xDc}`xCu>O2hZW6uk8MwkI!Fi9#3F`u#7o! zvn~UXIx}`%P5=6o36q~hruX0c+N_0ktrslj<`#N;Gv)sH9adqpjyzkb!$+#=$p)On zD(eD&mBekywjh~hied1|0r!Nl)~7s|QYf7gsYGw+CJXN#vrU{2`mp<6;C|VZEPzjc zao_N=j77sxu_d7IdyKz}4^(WrlY3>&PQwl=ah=PuL@TlN40lSfcDROuvV(cN3|6PU zT%1058`8a2XMF@M^NzGJH0^J(>Igka6Ksmpk#9L(JB2DXqr`4`ZQe=QNeVW6lZP!> zsc&2}q!B&UWl+Ue~X`G}3tQFiyt z5}I9pqnc#p;kE66)cXn0^DYY>ObDkWEkb8BY^=-+i|IFo;(Iw^i8x4%eKlBp+dxM0 z%6Js+IsjRdxELKpwRjN?FRZLS&g)BG_dDXXJy_%r44&uzzdES3c>9q z*{&DR5g>{~9_FYFTf_$Qu;?dn|NKRDwXS>SoxA^?2{f{tY(jtQX6o|*ATetpQnOqP z6upLYhIr4nNR~{P|Ek~rWHD#Hqa3qqewLl>KC0t0$P!*kBNMxaEbYtbNnpH$PPq(E z$nWW#WQeII4)Eq=RdLh;PZD66OZeBBtw(N8`C(5)1YoK22v;c`_OO3Sq&`CxFB=6v zCXP!hQBQyJIxleqxS~r5;0b$DVT#mh(#(JDxp)^By({wi$cRVh<_b0dCaRb1pM!x+~#*B|5Hh~k2pYjCvgqbhq? z2vAe};j#OboGwv`;9^ONpc)s$cgo_xXMiUoC}Ceik&)}FF21+w`Ab%Uw>mB516`Bk zNxo;Aqpz@n7Jo#LNc!E`rEO>4`0+hkOSP$wb&(Rk(e+4P|5~cXDf2t|6Rw2tz`F@N zodhvkma(_w0ot8b>c9knC%Ej6in_v{js;W$AM|SuO`TC!_xMlUi}%xgp&<}gKNrUL zBTxG|yn)iDG~QdinGrz{b@z=p9fiz=;Tq1^&BhcX`iya<4vt}@l>x-X#kSMl;^e%9 z314JD9K3ywmR0;UZFOGuZClGc1eadIJ|6-4dI6>>~QhH^LU?6Dbnea~dK(iLRANTd7z^BFP4{K$kTJDr!g{*3b;9p zO~zKaM&o$iliVfWzd{uYih_c6jqcVOdjCAnD}_#R0XNqAjPXt;(4^*8Qn$0ni!{T@18-N!LY;<7*oBU-(ADn#+Q8jy3C) zxe9A-R{2Jcs0UL zhhgMF@w@01Tct)V|WCsdz4&-q}jGQ-6U5N2E50p@kj7F9BSg|%EW<{a6olfJ6E{)(EpWx1a) zt+UE^kbDafYn{@;f3iV7^g{X`K~)J2Tg6Ap_m4X95TZeUil#59vwhyBEqQfKvFKeq zsfF zjk4J1fVp$7t*+oeVu(#G7}eH8<5?VEWWd8nTb)Y# zRxUI&$G++r^_~jNu1@&bXN~eRbge;cNk;ip4{g7H$t`-9ulkiBcuN5R3u(J2^6F+ho?&t&#+Gju3*obE(~k>*{1KwVF#0Rk=NLg zp7YMu!Zd;>t`HE3=z)bXU%HevM6P!N|AAqDbpLsiTQfSI!yXjjW(>V?IQ{K!>1GdY zoZ1Xl2_2{K#lv@cMl4FRQIVm1lxGmM@rI6TtlB-(>+6@pXXL#Z<5HX^OMRCgH#DEQ z-fbB2E}iU8-)bYlmrFwwd(&{;yhX_p{KR#a17vZu%l=??q4|@_yBrEpi`$vVDpq#R z8yZip5FdAy;lO3KW_De-1K&Sk1WIFrZ#GX`bg<_kTfz8r81v*8>Z{NQPt46R+(B>k#}9}>;-#kh9o3%IW}3};ps_-Ma9|c z?-nE~(=;>an$_WF>rWWBk@Ddui?{tReqjtcy_Fd7?B)4(4S`idwHAv1m^Ui?{`%?F zx<8JOd_UL6eV{{IG}jK0fb)-uaz!|qLbYeS+I?P?YbN$A&0MV08*Wl#L&|GoH+AGyoi$8i(ApMN3lh_nDKq`H`cgk^BRl>^2X8 zM!_gH(z|YWk#)kegP_`UY@pE;GorYQrGk#es9GPLrB?+G+1QI^$C;B$!4Wo7I5b&i zR^%c{Et2$2|#k$!xwAGl#Ppi=Ob!kGkA^iwh zsZ5m4ZzpCR`W0-gr8BO^VxqDxIla{MVmH@Agg2BhXtg64eyQ8x?Z}c#J7oAb7;~Wo zBM`&*MaJ!${r!qjTb^(p8ZmuEw@Ys6?!(F2G`8RaJy!WzNnar$zQ*aHaS)dX<8-!@ zvH{+HGpj!^A5D!|>56c2c0jYTBn$jB(`jJYitpBK&%=Ql&MAkepsdS8vYx(7P)ynT zCdOrftY@CVpUTsQbi4?Is$U7TM1j6I9_m(!<7|QVPOH9%EWmZP5@tIeZ#M0!H+X=z zD-!+OuNdqT*y4AH8!Cd;xXy3Ts#Z`W)tWO;8cvO;NAB;Io!+W#JBU~#LO(-RH+|;1 z{^gw?xv^3MH=1)Vz8_r7zM-S3dH_V`l8A4Z>rkx7Y!Ui%c2_yuQS!WGA)*V+CJ2m_ zWKaJ!S-It$XzV6T-`NmaF$|^|P?J!>2bmc^f$dJxLTQYLxCUucm^R$^2Rs;P?y|mg zkX;56*tVi4p=rwE+r)tKt2GK9R-|f0-3F6dc)0;YvxDJvM~6@-$(yh~*U6UMJr-yD z(Z@2a0CH2-xYa!CiuawG&GI6W4I^kmL|WYsh>YX=Yb+ROCm(^2R5NCLvKd%>gyz7LbN%KrqGMAGC1NRjM5B)`6)!wpgR-4Y><1qUiY4TfyD-4u zG_em^zeqYf_q0C8O}a=N{tmPQpSf>P6<pSoxpxoQMhPwY~({EC;9A8Uh61}b4; z05>P;>CTH&7_EVEqVE_#l?u-KY+77k=7zB2 zRxx}Meu=xIw(OoHH9!L>^c+|BRT6sgFIj<4O#$`_1Nnla=Vm6$rRvqAGzTSJbo2F{ z`Q8}uBzUXF_Q%;1S>7b?bQe%i1z~*&bF$+06MDZ%KcZ;bi^1om^sTPCv)bP<3qM^^ z==xE>JVRE0XmGVz>TgXJ|9;9ezYZMKBy(N;+0+rOvk15M8amWs#I{)1VVS@iHt^M! z1+sIlm6qj1qemvR*iw8fJDQ8xs5S2FCxmY?Y^!=a24kJfh)Wi!<#$^)FCuz$X#Q7Vll*jx&ZHeeWjdy3v`Z)>5P(tzP6+) z;IV42N3mXTQc`q?={2UFdE&kQ;Y2@MLdwXVmw69k6W=#x(6_ueW80JfRflpdmY>P)m^E0{{F*#}N)F!8+8joOYSV&AmvrN?h)8 z_0&-yvtTHs=|NgfNv5&|hKAfsCm*mUX2Ova`;Od}E7Q7GX%4qM_xSr>m*ZiZa?Z_C zXczmx^d30l5rR}o+e0NPPb55PuG8=FX? zhHq8J6(7{VdJK*E6-ir6w$c5`rW65-H<~tijb8m??+cw~&^@S9;AH#Yur@O2LhuiP zi@jNjcEW04Fc1OGn?Kt;PrFfFzJbO0ixBkQ3|4|Wu=J*KaZz!IF1W6J1xjp_IL@<< zX!EHi)7E_TQHbftd1!06`51^}8RdcW+I&n#8F-9iqQd=XQy9bJ9|JEDm>tKb4Zdkh z`iwg%lSNfB{DL-a5}9w(XnW9c`g>Duuz141*U&zm5D>yBVM%YJKJCUi8e;8OW&5o9 zX??39HShiPXHzTmlgApEpnFWzwsav{9_3v5H{CLL$oJ~)HT!aG5@&F=0p}{~ts>X> zpDw&{$0x|oA)5Q1{_zI`uzvde6?^=|< z$Je_xYf5;mng`x;KXt#=(#bqFpZXK?^#n`>?|__bsxRzqnT8Z5tqrW^TxGQt3+X8=BcD~$1 zeALJh9^zgEjzU(cJL*%GhU|qhmI*z#4ij~cAe2?BfLWyURH8(;`krZf;4{#AUS92Z zyyFfDB1^TtIR$uL`kaWQ3KDp$@i>98AcC8qr-uw82;5@~kNly}=B&SceWpeVv!uoN zh-Izs(s3#m{=xfiQXL7tOoRWGT@Ueu9oKggcU3OVLy?N)y($DnKAV9dh6LBG5&iWY zJ=ayCtoUeIAFlDfQR4>t<44^=FC%yO66_Ty@@t2#Ov1nXBE4@!Tb7bC9{nj^4Mb)S zGEdW5(^|XPqq4#mR!)^1JjVyw0D~6YYW@UJ8=ETOj&Rs{@6A0a?>f%e%2K!=-vAOU z%|m5{-npvQ-<}x_K!eY}s|5`03aKOyRv=ITW0=?D7J`!JVjloX2&CW(rm7?VU9EKSf+zbWU&h&I2mLZ=16R$zN!Y1l z{8FbjbaDv0UH0c>H*QW@6Accy!i1uQ`SZwOXD_jd9{?KQ-n^4>TwtV=+Fo^MgDssi z&EA`AqEl6uRp6h%TET0yy1OT-9G0B#nI5-u4o~~FQ84XSb)^l6%x28BQ}sF-I$w56 zoFVm3Sf6OJ4* z88l}H5^u6QetrI^9~ZE`9Y1qy$c`p`)T9n)1#ZE%_e3W1OC%5?GsgamPnayGC%LWE_?NK|xm!#%nWg z9nRs$l937o2>op8vS+Sgh+EL0>Pu-Zea(nOlz-RPZ+bNly2s0VwcEewDpo|wZ#9H2 zexXDzuNi=CPUWUJvcF&Xlu%zl%RP8dTqH-9g%x7n1J;D-w=43=H*?gFG^(d(t>z>c zeAG!YCrDO;b=35c=mD@M*L*O}SKcx5?LhX#-`w$*Ct&oRYN8SXDuM9SdLKE?zRJlT zS~nNpEe9RdAh1l+l>ML)?_P0}7c_?wlQ5(rb4womF6aGdqf}MG(eY+mlH|#X+}r~o zrW!N=Lb&7hZ8WrlZh29`>@3H&4YUp2k=s+C?~b{w#0aWEY>slV9=|5G)YU;0Jn_q9WzSK~8CgV+~UV(DD zP&{ir(9o_rp5u&S^lKDzK5KZ&BJHR<`6rBBr;ozGIEZ5shr)>vnEXkP*Av7H> zQf_1bh|q}BFxhUYnflF+E2jQk-44}z2FtYfkstNsJ#{se4vpQBHTt{jIg=x?TfsP+ zrjmTRm8@OWz<%pLRcFnJP<8hT5UaCCxl>uhgq z*uguZvo6`AE(_h%wbTYV<87f6&t&~Q=?(Ev*;?)Mk=_FeA-SnEO{Hb#X2N5*ldZ(k zQiGd0O~PAEhUCxOp4c7~B5W{z@oL`YOUPHwh;1-g^u!5r>3e!4>l6F0 z$ELLKSl8XJKN@-Ra2xRFPM4$BLlm6}D;cqFrdMS`CWGP_8S?Wpx%v`yu~mo__N6~Q z%8W#F^`E@9O5WqVq2z<7d`rSrv;P2pUOl?`BUz0#N_Lz+3_x(m0(mAY&tXZ7U%I%u z`C_mBU9I+PD@NNHk;=4^2E{b*-b03$@FK@AlAL2~0fq*{eB}&WP}Z{<_>h zRQegTSETbbr}~MA^H`>nyy{x&`xRS!%$#q)rguV;+6UT?i;XC*9|d(BjY(|NXB#u^ zRYlvvTS#c>7Gm$1zUX&?Es}feD4dGlfK2_+lsB;FLw#4|)oW+}tszINL~>HrcCeoZ zREG4`;I$DBJHAuKX-;T-?N{7G!vQz1+?hCwzTX=07mWaxr}JCc1ZT8*l4;Rb8eFv} zpFH#It^(&VFO{9+IGy9ElYu$jmZdnsxig2RwQ61F8|pE$H@O8azofLCQpW&2D#TmWd7wbCe%gS@qU_$Wh^71IaGnQ(xnA9%X>_9Dc=js7L}QD^<7ny))T(h zNM4r~I<^P9>hN$A@MvAn@+IcKQ7o&C`*G^eQ8jSznydZHU~-MZVLQ(^eDg2w7}`Qx z#KuTjNbh)vjzr5KvuYt}QdWL^*j2;CRAFQ%ocdzmWBdQ1=*r`vdcUwDvWM);2qlCf z*-d?wHAzC&DQot9XC_;+P6*ivAu-u^V^<{EcQf{Nn6ZpumVWp5m(Pc}?%emD_nhZE z=Q-zXas2O_q42c(;`C1FgQEj_gokq$msUo{FDYtVBN8HBC&g- zVLhYsApE-yET|vF+WXC!@}l;UR~G%pV7ex~VSs(>bdGuo{CF1>6FVec3y;`8fK~Z} zu#Yj1Fi5v3+nWp?ch+X<2i7tLpy}h)88O;+K&c;y!&uLMR9$es_9h6kS|HH9Se!yE zU#pWFP^XpJkWE5jA0g%wy6z4KVo|2C16SLDMm|SwZ$Rg*gqUxOZf%szBRK3u_&M=-!gXO|0R&I`Wivv`S zv&qJQZ%nwK6I?$+ZA_olcxf}g#GUhqo5T)AJN0_btK!R=2u?WLuEm`fuj-Q{9*Mnw zu0nQwYuNI|IbRBkG(wIkcw(5j*7nB}ZFRc-_^`Mx0L#aSufQ^~6tUi2eZm{SMM49A3 z&Za1bu?6I;X7=ulNM7^a`YjiI;UQpZTo#+!5nUV(UqsEBQ5a5%51>;va#1U1u?HjX z>%4M`>zo6(78Fhpea%^&{H|kL@5jaeQT=6x0f-T+weYzRxNBsPhsM>6u~yj3<9}3q zlt_y3wa!b^muTSo6|EO;3hQFm3D}gr%ivuEbo~ZzQ3p#)o?a~SB1H) zShw9Sn4Al6NPpg=ng881;oH8Gw%brM!?R$evVK>o+&jSvb3xoIihZL(uJ~opUq9k2 zv1YH9R?T!?pwI$ncK=b4*jKwO{!@dk&MuWaAJ3s>FiW2MF0Ri;`q~hLjP+`pGMq48 zd5e+xgrlZX|2DKJH{XicZC{@ODn8subXq6Xq+h5$I67Z>65Vb((w8~I*zi7>Q#=BU zPP3sK$BYAnkJzLPPxwZo|810wjZgvq_J=Do&xQ;Sz%r+G|EM^EdA%cqHxWVJ zS^g$>8wDbw2=PQq3PUFG=KljQ)>CUngP-7|YQEOI;OkpY_bDW8xl;f;a>1<2ANUGq z(;?|&GhQqKulNjN7!mf9J``cX7@d2B#Y7iox}c zV4N0BX=&(7{{BzZUQdRqJbq;qwFz3Zu#Dz!VH!XmjI-Tohi)o{IV} zmz&wg!hEp}=f(37T&9RZgqF6|3ropLKhFQ$A8qFPK-1L%X{ZRw7iQ(@U!209Wp=gR z@07u9#-b~oqkUX0zMC2iCq~P7r>a+;azO}+u}|s~W(n&*- zB{;1bez+(|l>OQkubMC=a`ccUQQz*Z(%}vyJwP#Pq$Ocw@yD`0yU@B+I^7XxhvZm= zAY+K2SuauZTbL0xBbz%gZ?!|2>hvF#ZZfh07JCozzLR0@)eS1kpx{>#7#7#_Nb=|V z1q3~?v+U)!gq>Aab3+pCL+w4tgjPl6gCuM|#hhh~?tMwuXbEleJ!fT+;0ACyoFFNl zII${=IbFk;d_Cquz09Brf6&6Y!szTe;SqdjSx_(g0ip1ZYF>V@mHfLjf31K1-JQ?~ zFEli=#So+QXT064CHCtYja!-T(^qrp#3)oezhTWscZcuWhrUVy4G$ZixjgNP}>dT~DmJ=!+UW&=-eT8@p`D=$aXkM@7zc0}_r*h0zt09G582=@8 z9gKMRvWvX)dRQUHI)d6UVsgq(c_2>#H)2S|EDXJY*Qb^a!{U8I+aO-3w*|fb8fUqhs z;e{Si((4?E@?o$J>U|V_L*}TVOpg{tA2N9lD5_!{W$n-gd?L?utW`9S>5wLD|GVdI zBx(ENrcC>&a|U#V?d133^K9);6x;iVStP!4@@H}{8Wis+UX<{-S@d`=9?*3V#Kkel z@7L}h&LR3)+loSI4atn0WSJ-K1BbwI5HJ1E2ChH$nDuW>htovFg_|eKaL>v2f#BV6zc9RD(S?Wv# zwFWdz?1+vO#Me&?W;s}-LXvE#)4{=O)+%>l$N+%NQwz7p-!uUIu|i=rQ42>v2UMz* zXLQrkm^d?8zRO^)5$!5i_f;giP{pLEJ#(e~+P8mHqoGp*6rc_$y9$njFk30~6|y3A*ohu8Hs<8XhMio2)A6-jy+jCrJdUlJ$eJFoi78lLGp7F-nh)( z;(1}l0Z$k`>nVVb35sqeZoTII8doO-$?YWg-Oyo>roz`XnQB6uNW^C-|QXH zF*H$*^^RBZxry${&-CQ$tUaxEQeQ@!-TqOzgXU$QeiSrYfu<`N|KiY*|3{TPlLeiD z)x#1rXCRcS(XqEt7gw<)Kyurp>JL!!* zHn{F)crXw&qU;#tGJh-Ra|m11EUCXPE{eIg)U7LsiO%I$bJD(o`S%v`;^%IR#dKl& zYqrq_I|(gA&4pqIc5;qyGvA5f+_BMLKV)2nVy9$n{FWhe!Uyq(Mw!Vi2<3>UVF#OS z1(BTsFd;%MD|4fX!$%2+y%*Un{+wg@{lAofkb#+mm4o>2Kk4sM_9L|$?zLYb%Kn-A zft^fAMmbEc8*W%Nphz4q%&Ur8f8cITx{FEnKUs^U7dRWV80-Y#Pe7PXy*N>7Aagi5 z8UorgEu|i+U=CD0C3s@LD58p%_MWqJ`_F+u5%*8ANexXZPVovx$&va=s6XwgMEq@X zaed^o931rvo^4Yc>-NhjdL*z57Z@MY%o9+?;T?+CJ;|S5gmwI$v{3I_2pU?vQZrNX zF@QaS!fCS|X7M^KNp5~UKQu120ni?rfnLok7=*~y@tc>WIB;Kr*Au{~l0t6mR`IVLDQraH zQ^>{Nq1J}IW-Qv)8PPBcAIc1hegBz)A=9oTopr_$?2*yXhzR^CgdbAge-`PVXv~3v@7#tdQ%aH_!d6M9t2YoFyo>WsrH)O3bkPB)Tmz@L1aN zB0iRTwhn_Jh#i!24#H1~L0agQ*$gYYGhoowSy-T3^>@p>sq;Bt*Ci2J& zV0wbMW`WV(2f)oilhW7;6Fo9_MwVf%w2UMEiPqo4;z@t#hAC z8=q$|nfoxDVIPVQdevD&1NSz@(*u*Tail$@J4!G+K3hgMto_zEmmecp5Y+Mb3#+Vvhh$o!nG&r6cbe3{> zkf(k}Di#Ck{y?wC^O5vsqdvUBYrKdZPjp7*gC=ZX zZ|g{3%zA-AVtXthS!LSz*_{^#f)elPUfVzMbmT?E1M-Lt3)3v{zF?K!{ha4R3(*N4 zMV}&4B57T7fi2bs80BLP4N7mP67~~_0kMDOJ>s7Jtjx*A=f>;#^rC`ND~JUEPDOc!fd(KSYQkRQ!jseO9N5b~cr<>licHC3^@bS1 zy5-s>=mwePjoWYy8;diIj4j3HEd7E<%wa3IDD7m_8Z4nJ@tXfaWYE4vCjdetZ+7IN zlEWNUhNGXSzT$$5lGCzr%M#B_wRZ^BLa(eU_XOtWG&&3&3f7wTqp*>t`Y7&ggFwe+ zs~YoG-m%!{;d^i#oWx~^H^C}}3UWW5@jNgV*1_8i;n(@jMOPH}eT$%z;3l&^aBQdO z)UDd4x~Y;ZgRj)-;*G9*qnCkYzXi4f#WFeIJU_^K1ZljUF8d(kZGE@sKfPlOKI4sy zL>%M;3Tw>@8s)bPyGL;TQeywwAf%RVs~LFxT!aE*7-gy8T74yqL#AO>npM}eoHTcW z-g7ha`nhNZsxl@638XC|t`$D0kKsVWdMk&QsQ-X$C*7qJEK>>&Z;N!c6Be*#FR+0? zvIb)&Wa=g}TN*Z*-#l9DrU^-vw7j?O zX880aKX%HY0rjJ1{&T53*QL18QZFG1O`(<>Zcc&9dp2>Nn+d73;iIL&fL@bhXy&W7 zppM$q*qOb%wPXyVcC!9Ev4_5YZNE;{S1S9RTXlVVY@ZY*_^m`v(n{*a#nj&1>`rae zGBC{0rr>@Hi^ShAMc=@Mql6na(f7f#n!SN~&BUiRj@~DS@g(4pB3oQU{!vjFq}D16 zeLoZ$jSAWzAz<{!0MslP`wU5KT^l1^lEuRYU!faE@*_QY5E>Ey+t&VMg)H|{QDw8{ zN=#iFsc7yeuV|xzw^8Qd(&(peRS$wt???*eLC32sv1mb=mzGt5#_>Z_>nkRUXbudd zka7n|@j{{M1klRqS)X9wDk7G-IWUI^d>264D{3wnmh~kBS#o`Le!J84t&bWd;)V2MFt1IcHT+{oHhVOYA zh7il})dUsbn`(ChE+xij7iMQkNOZreeu)PmSEVzgskgO(8U*+m*)akVi+Du{EDBR? zZgl^TSC~Ve0nMUqHM7N%y2sw8!(yMlE*x-Y4)ND;A(L`-1bd9vAm{kACVmWB4Nb@~ zXAudWtrUPm5Ic}Oykey!ts3=6;PaQ~vh2$_ARSn@g5T|SBcfB#i=oGhKNbfc$S*4O z!~TPT{L}wx(74Y0UU&=zrE8NfM38hybX>N@9;ykNM?LV$a%UIq#G?vC4ooHnR}Je? zCf+6>p6fnHAKRc>EBoo1Q6}~6gRT|DRL!#V)o#89?m?0FHDARzM2;9&+thl1_=`!fH${!`aXyX2OS+3#F4Qe~#a!Fp$JNh+Lgj+`z0L=%(-Ni-%>pM@8n zx=>LU?il}qRzG7k+Iv}=bxz$}Wj$p1xrxETNz5+ICITxYX1T+YmU{}aKyNVE+iLs9 zA5iFin&NfHwB9xza-{F4Ke+K9PR?~u^#ZuXnS7FuCn58{cmJq(stE^689YK$TMz*U zTBU13ReLrM_$ZSXYL1^Yv~xagJRH^Ym{D4K_FBH4?@?1?lk$PN3-E@FvIBL>(O-z8zAaUKV;`(LZ`FFSeQ~RR6cOpw+?id5tgPq9% zM9+)nuAlf9uqtS%<>xWoj~+B$SD;z2?%EK&9?(aaq00Tr_ya`_5jVpWijHAzT>>xWHiKa)j$%C|%; z)eBX#lS@Q{B}Pbu%6@kzCNbJ~`ztc5AR&!hxuzH|BiM;AnI%gN74iSs#1U9}&hVY& zqC=ivihY<6E%nVAkDQa^d**e}DVU(pe-nfYT+4dLZ^S>UBIRmPmuO-yPT>s55YC9I zVe;hS&f7sXP|0mJ$o$UuxtG?^RT?dR*T)-tecl8zdh+aWKHiuRiIUn}kXPW!w1l zxQDF&quM_u3(W&vjY!)Mtl6kK$a@bS80VjS1gYN!j^`u8Un|#;vq?7?Sz_)-f35ih zd04Ls8mULJDa1TASapp{2yE#HrvFFP`FV^xj^wR!IKhQpC+~%Wy}=)hA>$*+{Wzk^ zD`771)e}bVL{`zKiFPMti2YLRWoIJ?LTH&+2;)o8c$KYlXT$--?mTNs%^j!GbrsPt zVmc|5Ai{$ztX{wiPpQ3G9bEwRWSfk}$!-~m>WnoEU-P2cDG0e3rLfQuLea9ZsOxv` zs4*AyzwY#)V%jzA_Vf6oR__+TS#xc(^6F! zPJ~Acm-s!CJ9&S~%}ZRcNTmX!&TbPF`wOT~1(g;qiGL(MN?*>=*q@fuK`d9CO^LHwu^%)Jk* zgG$&eaCVLITx6HMev}YpmCV2gGW^dhE6h@Y&xcu*K8`8IHRuTY3Y94dd&kWNUQBy3EwnmizOccas!{01 z4*u98wW!N9N#8mKdxuT`s}zBu)IignCJ^4BWO-so$UX~d$AEJG7v&aV=<;J5@eZp% zZf0{84(q248hHgPiDC}6G4R%D|CyevWvV%b4dBBk5G4cv@9<@QP`OS491%rnFyzKu z!kL0jp=zBVtO9sEv$kTfB#0T6{>N%gV8m$KN@*?i#N+O*$*9{!?Fcn0#i;;S;c?~GQr zz15iZP^XEBI-+0m9~HA1j_Fb-MeU-Uk#ZjhS}uqWZ?r$-6Ire1s}wM1#*YJ#ScB2I zPRLv@zNoV)*$Y1()9%it!UFk66_5DqNUQxvX(UX6$QI@@LlF%Nj?$YNXJ^0pdPnH0 zY7NBa;YjlU{oan{qci2*^MV8b<>gS*VT?A7azE2ewO} zYg7q_ij%m%a)IlgB?J^-`2;5gbUKrhD0hN9(b^dKmC*#DyNYxM6V{#6jWyg}B~c&g zY|uHs#eXQ~|PjD!u$$rh}~u~pIqI1`+ZICG7!p7*q%b}qQV~*LF=;5{t~8=c`$ylGT9+Y}cMc&LD6A%mzpmz2(%$EhDPchl$75ZAzZ=Qg|G>80Lbwy0d(ZFttX+crf4JTsmZ6 zr{6n!X_nZxtUZKS7_Yz2$`Dl0FJy1S|JUP`E*g3&Ej|Aq^7)s90*0UjL)+F^KrF^U z{zs)U@Pzu9vrMw4{e?JoV{W2hSlU3F6uS65nk;qZiMIyaX)mQu#jIPFIY@H=#r6Fp zPw!8+NPGMauI3#lLDgkb_>yt=`gGN&-k`8+;5=FTEO{V`0+;PkCLwhQ44{=4Izwys zt1I@QY@20+?yvSOB$b5qB?g`%M%`b>y6<+VY*r76g|X8MYPmElGT{Vnh2vfpeo0JE zQ+J}!H-t16G=U?&s(Ca!kWG=r1srraU715FjXAf#hf$D`f_+|pavsqoh+!p%k^JRl zF463y<~p@8_Df%TmK(#`jHK&Gzf|9HUdtG1G0e}7b<}#w;&W|s_ZD#MCDV*Q!jN)8|bw5zk@&nEL%umICTa6J_B*IR?QeJtP?QLNLVVq3HR{J;11> zfkwKZV+w=*3M7^y)}Fzr?lWiGn$NCy`*v<3S!%mn|IAq)*KQ_~A3FoX>LdAFG^EJz zlO;XNne4}{Z`W;u#q$mPkd2tgT~uvs5@v&@32%9qCUCP;j{FrWNzw*hmJ4JaIs&RY zDD=GFR;nmszJ*c>*zo%Aw3iwlvP)s6OhBb@kp0j#L3w;OPO$%d9n5WYK-#@gt@6)h zTI%^PM(GEMZ+txG-mmW!V6_U^Ebr&!&o4igSoCo{&g_x->(Y3b{IFZ-^x_invI}wn zhBZVA1@Y?Dif7wVZHY?HcTzPO?jx=nyfwSUpYfb^x;Ag_gf|RB1Ao}$;}LkemSP2h zC;R^r(|9-IN`1NOkl3rnx6&Tew_AHDKMGg=wcy^)9}Au_dHQZTLS4UWyS^{YgL1hE zor?31QtaFd!c4Qjc3lgx^SZFrB(8pL5%(8M8ZBN@V}a|+l9PJkC%!~#XQ!S5tYHC@ zg%ijLJFH5(ANC_$3Ol?C>PwQ+Be&lqlvQVD3~S+1mlnT^`pP|#5MvD?uMzY&2w?o3 z*k@PE-ASV2!->A@Gsg+5WK#pk;PQ>Bn`*<=ORozaNnNt?yD@5NP#|ljTZiXg8zpfc z;tdH2SZz~6AR5LLl;r7o-`ADCWy1RUy{A6VhVwDNG)d%Q(hZ^nbleohOY;3{Z8O{3 zo)h|V5uGWW3sry<87RYiXP8plJUG&V7Qvi2FoBV?*6X`swe|y14D*B&L>t?P%+S$H zke~wM13o*l-|t6ws^WR_E2fjP>N0KCE3UaSU_ICy5aAAM4`N|(&3~r*kcyyfT$?>? z$Sr3We92_tC-q)`cub&Q#y_gCWGS2!g&8r%2bnkQ%a@jj;nVwyTR8NFltQr2Sr>A8 zAU_bjpF72ZhBezJi<(DlQ#zHU?Sb7a6g5k<^X=zQT?nXSg+I``Ssxgeqx;1;V(Ox?L zU832>D2d;oxE+{j2KqqKdKtlzTEh*-%7E2owm~Fr&Hfn`?tSX&B$d~xBPxTb^1X8K zJ@YA8r$M}gqTTuN6w(RFzFjk1wLQaFZmBQ&^JlzImhvxz+LWJaj_OwY<}HIh%xk3! z(|s@($$5p@XUnj5lJP%Xb1u)%*Fu@{r8)@4Ye@RNzt+yk>wZ+(VY^R_IjQOPraN}! zTy8eFk8*LHarL(^u0u`upIsF{MOfkvfuaXS2*w@6|2*l0P}jKlKnG%RGyM!r;@E6i$NgYx{y@O}5 zZHbu&&v@QxX_qqTD&L<8^CNA`F)xHm?Ua*l6Xt>0=O=1*3L-+@$*1h;;)$mfY0hFC zmOShOtaKidChJ)F=U=ZUHayKzi+ac|b3lbQ+npitUwLh8hJ$`Hi}C?T!OoXv8+Whh zsMky*AxQMBH7BG?NM?E#Havj_gdFb>@?7L;LBny@1&|dVWd9UlmC;3HY5}fehTYG9 zF!^MwU(kI}Kgs+ZF!%jJ4tVuZ0Nikb4rlKd;c}Fzz*s(tM+0v*j1RTT^62rD;4BNl z&ISsqXf=(y*duZR=6VhIJTeJYH;{xAA6OP`;mauXNNv`6Ke?#lJ6%r!6BK(O*hg?$ zFdXS5C?^d)>@%ioYnq(Ziuk$EZImCcRIQDF1Kb$cTdV{+thg4zygZBs$5I^nwX0^> z*FBIyi|}0jxFLv}#0gZ|D}d)B+?IWRf?x{LUa>xmz461(S*)`&+4N(%`elwpU;Qfpb zE?g6khXM=QB9gb&>*De1j4MvjFm!CbV5U`DbEavs=jV;z@MFQH%KY!Qt5QYC@QId9 zuZ@>vXSYc8q2O3TtJxNJ7b`)DV`XpIr%hoIoP07fsH*C(JotGQY-9rn+i7fTRN$Cz0umY~C8A|d zf5HgaryzoCM*Vy&hLefER@78!6fhVDASFacS=^XHx=Agd?&MW?@{oW`(_**mJ4>9! z;GzH5ozp9h;u(l85JBdk-5)YP&u@KxH2&-#6^0dyKrIA|5jB-TyKCa$N02{A5|^c^ z3IbCV>cTI=12+tSVXNbf>RQj->Yclv<$k9-^U z{?dQi%|G&Kd70VhvJ0CE1=y8$;F0TbV z1M_X%$N#oLS)3FsKy)8~x#z>1P5(So?+?z%Hm_A4P(ox?;R;-%0^{$q)lv=qoiV^> z8*{@k*V7`qa0}TRAJeHY3S(h+Gly}f`TNHdky?wq^amPeV~)lvL}&W-TP%j-MICw6 zVeN3e(&EK`RQ=$~KYmh|p(Qf8Z$7(xTG>{!Ii?!)zPy%y(xk{ZSy29c zr+Q!w-t$F^sZM%=E%M#P$0-hjLGdQN3Tc$`Lp2oGwmXS?AXjAo7*Ww19(Bo^iXdXqPG6tbBC= z$k2WTu0zKHN}4RgK;6?wo#RX%M8{KTZR`|w^kv&F<;C_p*ne&2r_D0*(xyGMrVer`~D({ucD>4v$`ODO>T1Edu$Rp?i68l7U zP8<#2Ut5L5!zv)641TzoM^9bE+Ku1=+!hq+42eKesoAYn!jcO({ooAU z6<5MFtSx*J?#&jyd#JW!E6AbU!~;?cJZWI9=f|_dZ}g>aIi5j1Ind#%OrloWqR7*)aPuF-=KY% z;vCEwa_KipmSp%y7}w1VnFRH!Qc<|ZOGLBo%$$yR+k)CEzJuCf%BUt@CvqKBG}@qC z0Y5kT{G|XTnDnh_KcT8Kil`UlViFcS;5e=y8LzXq43K`; zjp5K1l4~jfbY(Lg7$!1vT=cN1@<;RG#~_l_Mkilu0dg!VLt9W02pZ#tLBJ}%vtA&u zQWzYtdwqQmMf;UnczeOfYl3jO46Y{SU6o`-Bwhs2mGFoi69)2kNVL#rGpv^w) zDoh>o<}WUua7WvXc4S=eOM`mD2*} z1g!fJ(ghR)seh2LLI$sF<^^BB@UOi&vDn+7OGTX+E?IyXA$kuIA)0p1v>!5z%nFr1 zTwoF=ISZ%ws#g8Yl;AViGU5jkoh-zO`x=WA?Q0E;YrhxlVLkWr%Jl_Y`TAGf?05!j5u1Mw-e}ZS z{4P6mnUOvp8bFpp;wXIZHK(qYPSDQ$yLT?xPrU$Lek;>!m<8#=kf3PyG{gK>)M}-p zv-#}(T>I2;spJ?Pcm*4-w_;{E))i?eE zL}7pds5_G2PxPjYj$rM&^O+~_|BaU5OT6OJI#9ZXVu0*I=Gj(n)jUn# zDjErZU`=ERlJH*C!syC!0UCCRAa6XuHNRn>h4>`nU7z^+BI}{mujq+v&M3&$TWO0^s@~L5Uili_`7~QE!KsMK}1md;3e) zNP6k0`jUZ3<}Qh@tJjSe%a>)gHa+`fXVX>=yAQdlkb|SVgLVp|hwQ(iBDJj?47?yh z6pc6Ly2JKZ(3|o1abp}7;gGEGphLETaiDJLm0*rCw zA^2=?3SBm#y_QMGc`_Sz*{H1?$=)*aA_ez-UaqPU?s_*a_ORGF&Q$PXx+%7yihEFH z|3LAx6ES7oa>9JvDW4g9jGeZ9Hs^}Foluk5*YB@D}oxv*!^r&agRIX!Hm#jlIC^TRt_;V|aTnAL?3H|q5L9nOuk)Lk>v)|~qEotw<} zh)`QI7Z%AaU}kj{ASxkJ{3t8XAuI;hus`hVpk7TX<5w{4vcU7ayEIE4@rH}> zP_r|lO!y*7btW@$oXML2F9&R-Y`)eRRcqRCLusUqqCnbuA~jrr(kZ+)Q|kj;Rv^>B zx~&^NO@QqUbQ7Cd=>;y<87R;bRm-Yp6n!a1U$NuGmL$3ty47mZ-~ED7kmBxsF)P=`9t-U1;t#;&1;7iba+HNd~0Cc8ZcM&P|XDMF$&M{ z&5qxeUwqrSXhPf&(_n&dQy2V0LEYB=Qu#K7{e)`*kc?)L%9ztyU-Xs>CWXjX@@Q>u zdK#}j0`9~>{(cN6=$_P3Hlhm1`RQOZP)TdBfXwbTsg$;VoJBqHRw=C>8A|3kwcCLJ zxWOeeYEvkDN5S9;8oT*rR9s&hTkU9uPd*y1qVU&$LWgU{ck%)^U1hUIICr?y^+0T2FBz482D z!%VnG)Z2luX^7ekdzQ>=ap?+w__?-87Xt!lB5Wp!Yl1?5cqOl@qbadn^Ir0eO{YgG zS`SYV{Uv}$4S_D|`M@0EY_|pVbv*!jFGcb{{P8yZ|U1ju)CJAKVIQ0m$Vz=76;2#rKSKOzo3e0!Mw3KvWvX5 zD3#yKSg;4VP9S#K$Pk>We*rLvZ~JK0MhhC`uq$-{cwkn&93w*0s3Ef@ zNu$0@(mVSgxxb+J??_bY9H%2=%I5nQ^xu`Ld1?1Izds7BInoZfa3O-2W0SsyS8I)Og~wlaX=mmbjqQrU7RyCnR6pb<6u+J-oWB?eVm6 z+I!xrg4lI9zqy=1*wP*^-2_gd4822RBUKmZ=0N0I*Df-)I}l{pc@}kvIk!+fJcip$ z%#SmDr4lax@cqL%p%-gM71c67tq%4KjYlh+7b;3@AA(Dtw|$0p%*^L``yP9+3x!L@ zO!X#L2bYTG0g8=3%k3fdlZL)6Ua&mVYr(Tt9A+4VS5{@6qPVrD0kB%tWDu`P71^-Wi+4NQeJ^0Hg$a6(~*-T6T8 zNU*cvT6%3DFmF7Krid=eNbVi#9_gNi^W-cP`x96J57O-xOGrZJ^>$~vgNE6wTjX!2 zvIKG29%L+-59z!l513oOy8!C#8B?c2z3=DY@WWMs!Z8x?ihFEqo^=)5Y0zMCixo#i{R zkstuRv`X8g{T*~y-`d5}9U1Na=j%jEMj|DEJ$O~M@Kc50n2LZE39VpCH4cj+Hy zDByp)jgaV11vSY1F`}XYP8{tS(U7NT8gt~>f@313_=++0zUArz3mWD}CL~+pY%U#>QAW3WGwE7P+lcWa025 zy}yZPO3k?>G*HzoXZ3q;Vql3XNBgI{-hvseo>-p|FyLt@}Sc@^k|s)ctW=O>>0 zMNp&$g9e7iG)8ac5`zXqidGN=ak~AK!pAgFj?}GT>YPT_6zkX?viG3|-`l_^zmDCm zt($4GlnSf9It9geJ}cF88Qh$0;x=z?D;`CF9T9V(T~z|(VbXsof1Yvaw~2X#tC*RU zk?s&wo$XVdK}`2~QYJZr?p(7s0ezS@vpD!XZ5qwXD*D^1yo#B9SD%@7!XT@${#vwM z@@ZLP`5)Wmv@hEsoGal{`b`Ztb^6$@`~>Tf;N*^a*?rKwlNp@ncuOuk7%`pYX;MN1 z2B`H2AOt)ePtGAehhpTn%wvBF_{MvGDs9<*W1pthxU7j+>bT5g#=TnH_B_0Ot6X8q za?$QtlD$p9T9waySFr5o>WR~;3PjM!?hsZ^Bvpss5B98K{~y&4+9dDQf_YR=t7THx zvS_-fx}dMkf7>QCF+aCT!vXk7lQf31BDc58JN8}sp?bXki=wb(MA^r>{<0#UjeFSs z_|Z_?n0PyRTA_<)yU-L9GtX(N4(Q^zZRsOx(B5Vz81JVMfkGX7Ti?ol`-)`@x^n(D z;PM>}GYQ=qUtov)x#nkT7hmcW&TGReW)kj5>^XRk054U1^CAJ^^EGyP*`I>n%#BRRCa+UTfXXcfOz={_;R%`0J zkKHU5BdMvVZXx->=xqe&AE3*O84Zj#)yO{nvH$QTVphk=ks%U!4Tj!G@^3JlKkG3q zI>uYmKIy{r`ENVaZ1zbN>CS<26YW_Z*%7i`&%1rwQx>v+*e-5K(kr}q{fKRdDFx!n z0BbYV3OljER@Tau4xn-##uV&WR{A%+JZR7s876ec8G`H36M5nx|5z}9M#v$;r zeb3W8-TO=G7P`3`M!`y)Qv}Id6aI73TL~;>uGF)uEgcNR7ud~jtzWZb)BU+jtdb)e z!z9X=7WY!Etvh!dZ2t_lAVBQZp|7;kw7--jY74cSeI#jrYKq$!wTr^oRC=uc5Ia+M z{Yep4O6!}U0j;h!m;W~<6JRISk^f#T_0oU@&A3EQ;q_ma#F+4fpqv>jA|xk3!==+X3MNfw8WQT`{NTcY`yMUT_&f%AhXPtS{i3 z&Y+vwni+|^chw9YCQCHhzIYa~I%3gtk1CVfKWD`^Md+{z&F0) zneG+ydb1mET1>$o6$}TC$nY+{slT>NA)zT42pom=3``93#H6FFDml(nQ?;U# z;?H$Yd?DW#*96hW+W`{jJ9#?*f@d^g z$fzE!Os?8DJ?oMU{t@=g2gX1kK4JRSvNNhjG{na=8ECHL^*suFE!0uD#{cp`6Ek(^ zgVcKv)p!G)3o8yi+Wd`qmi=(av-c<=(K_*>j{)DVD$3F+_-YB+G&rJ<#MX|g?|%>R zoh9=3)UmB3xPRBVCw^3MP~Fo%Y#;8*(cR=Tvf}FV$LmL7_k~H;SLJgeOID?s+C>{Y z1M})*G47l1pG(4k{6crh@OK&W%MwS~Oy!e`R43KNRP(?$|4|wQ(&W4gdWteU@evm< z+4bblE==9!b<*nlGo#==pRIcimi~sklvXzRMN?39??6!|{JdXUUvvHA)~yc*W&!jh zm-gh5pS0PQptw@U7cF~`n$q(qd^+|d>SiGF2r(1rC1XE$mIPIz{K|Y`2_4fA^amdS z?7bv8Ds}Iyv~Jkfz~hblaWREOxb7G@Rz>erTLec7CFqnG$6K8nZ%b=)y`H7M|BGJTK&>xi{jc4*kBVtyOAQ8)02lDQrBIWr z)D#hf*q6ye!x){~CB0b_kfQi8VtE)ozlVQYGn)zFs~78YP4!?EV0{D`v2JfNt-aHD ztmalGIo8Mn6+y@*sJ*(^)~}LvvCICC3TKj5`ulOQ?}d6Mev1ruf3|>}m+$p<-)jwA z1C$*l5W;QbzQmiOCFryU3w%;HXbnWzfGLd;a{9_PJ+IkpO)e`_>H%EyE)8kX?slvczOz53X8WSWFPQYk zbibh(%nC-gs=)9NO#gSQ^aXtBFOBBQop&cZvjkXuS!DHPY%|H;_r=}_xD4wejZA~{Dw!=4}=@RQDIxiFKj5?<=B1>g5fr~O)>cOk6 z!JT(VvJ-##Q6E~ZUZ0M1DG*?+$!eZgS{ELzNt=r#z7a9ddiYJ9iaA1OSr(Vr%TB0! z;aPPOdoj)4<*T<7*EmNstb~pg)J+w7Hjv(orNr9ZP$I?nUcfO9#0M7CVko{7uPoxS zo!zWIe?vSFf4av;-YWx7w+YI>Dlo?qoZ6f|adepfIz20?J)Mel+L=1TED&$M zGTcxc$dmg|W#0~%kH-r*$|RB_mKC5g5~r6X39hR=4gB`?FP*=vJ6-OC;?&y~c42Zy z!`TR$7P>?M|H>eRtK0rYc_o2-cQy~TOdV;6r`5i%GJj19zDnu$6T1F-UgR}4s9Sw6 z7x!HBrGta;JC*6@Nm)D3oF_Q68*=LZoT1TMXW+ktvdld;^Rc*im5}M1aDs;M~C1pxnDDbGx`3ubhGcW9SgPob4+T z13z&tVcLg@^VvCN!=QFAFrTIS%R5~e>Onw@BrxNr+aB!6XnJo1$MN{3jiyylI7ECR zbF)=imcp1ubTa=znVJey_o9PH^|DoflP}hSO+~&|WGyeTE(r3QU!mpUIJCt3m@k&g z_3{^ZXOkee;AYbBHLMem5LY;X7<_u4Jdjo$zz1gA0Fwu_=AYKo^lI@ zJCIVgC$znd`3-G-;_48Q5Oj|DVbbq)O@ciK~)DiLj0DRX?r2hcHa{mBfT|zK*4bb{1cwy`WbaE z+AmGDK;wS&V}bc|UyNVyQ?HGl4*j720Bh}U;vd7!eKkE1*e#XvWGsucWQ>lP2E81! z30@ViDBJo$D_HO-%I_-6R!aGhv0gr2^xHIFGZGE}CqF@214zHRu$ubP+c8CvBV-I> zzP{F`b9E|sA4gqhSS~;-8)K)vdi}43BGPmgym-JRbJMkOKLxB2?p{ehiTKafy{E;H ziL?mpU{cJ`j9_)*yqrXh>F#|00OM$^c!y9Bgf$M=sgKipaRQZHue(h*@_du6S zjUHfCv%n(;vV10+bzQ{g*1WpbXR(Go4$AO4la4?<{uSiDF>^18E+hw!GQBTyq%6sv zhZW`;=`X8J&)0e8vZEh(>BO|)P;#KsZd1yRy(uLFs^FR<1B}3navJI}{{W8){*C_t;FDT> znpf;`q1&`!cR;5D-!=T_SVSLJ)1uq=wFn32BG>ej{{RH9N33`&UAOT+hjl3}Z!P}-)Z9ZR z)?SB$_zM1yzi8jtYvBI??OCE*czfeNgl)BJOJ@xXcMG^K4tiIP&Hn%dZ2gDi{{W9~ zg<4&{JI7+D`R2Ir^{LZVk{8VS^r|!e0KpDFV{5W; zXLE0#`Q-|6`HJoO#ucJZ$1mB#_M7;h`z&~lh1RF2xOwkoX(wa+OvID=n))C1bN>K> zsQ%XDtqjw+hZfERsJO1P1*NxEj+HL;;s@8Si z{2Pz^D*3;-_{ZadI|erV{y__|?GX!z`JJqGxm211A25Ua^{tdkF9PpL3 z_=%~UkNIhKqxx6Yzwk}}0N4)C_6hLa*M>CRY~Dw4Be-UhCoP=wS=6ZsEd_l~Uj3}~ zGo*YH*Y02#KVo|RU4Ay*%X9J1#aE1SUsG~P9^${JAKPzNhQr_{xbm?1?4KF0<~QP2 zZ$2#elI2Om>QZ3i-klR>uM?atyytITw5-?|1L;;;*a86NqmthPkzR~8As=p5;?b=s$#`cT&+j1HCbRbA2Ybd%;P{7Z4Ecs}aWfJA4ndic{_h}w9H z=HFoiG41bPWa`81+I7To3$0 z!|R}YEef&ZB5-?gT2juS;6Hk`vBgkyk~*<0#}!^#yMy)3EYquWHCkD5z%)r5&vCCX z-PCmSs9r&znZG)0(gj=r+pSk=a@-!(n^!c6V9po5KD8U%WbNE4FFXN|Gn#0TRON`x z9H_OqFWO`2k`6egi-pfqQ8^jinqiPD1q?auM<^bF`!iknA0NCuAw6zL{VVn&&Tq6@ z$2NI%1Xtrn?D;si_}}5{mtrqkZ<3P2WgSsJ%RJf~|O!-g#3k~}_{9pZ} zJ_>0b8Td8eTdxpmHuoEz+SI6J`Lmp{0Dg7(`{D2SBlqq5t16rS0D%|wSsaOOAS8YQ zO8vXr$j=pO)d=2jBoO!nmqnTitBVw_#l7mXB?_EO*>M$9r3Q*uktna z&Frw+s54xodFWlh&*xUO=@C#kU~}AaN41}E^EP~|qyGTG3BTZ++H65@J{9WMEsvR- z#dmo8w^vW0{{X=pzu=wP467!c@Ir7`pp#pf{{TFi`dKam40BV!q~mr4OsY?~U7jU-8#iJ*D)hCJsL=W@Pt)V=ANe-EyOt-S0T1-pvBsspH8AefDS zgN*g89T-Wcr_J1RiqU(vlLbz2GhLM8d37C`Q^fZ6RW zx-BUsv;8Ww$dV36O7y8M3?$g!a0ow2chZm;*OGr)1d#!PYq2d1+1PkMd_R{=F?gMd zO}phgh{*gZ-QS1&4Rrqi@hhEtzTY|iwZof;@r~o!oVSmF1f&7)Q7TSbF>@WokHZTc zT0sS#rje8D%^L5)eM3gmX4PZVqnbFAal!z9opI4>R|wnI?nOS*8oItI)}Gm{f3+kr zFvB5iKil0p@59D~~;ybMaz z=%=}7_Px{XmtUU##wAz=(HK?0>TAdRD(*E9!@uiZ`}suDOYs$=;V%N<`uF~| z;2s%e7qON#=eYMhS`8z%XDbmbcmta9{bkhZbC7p_@ZjU#y&7qD+DHlNhP?9n(|yan zIWB6c#VH$BmEva-NzN!z17OqUf@E*G#YTXs%dL75*zu>iVkQHpT9HX%(xZr;#-u|1 zFn+nJAaGJ!kL6NEkGN7p!Q^w>H99XJtpHilBHiL$9x{IRt!_W^BG>fC`yA;*OZ}TX zH3Fv4+H^#F@n6jkhZoNA--lwy-q$S#KC-QUP@l7&k@gSS@4|5G{{WaoMB|Rt<70Ld zQ%6dXsKF;Z`qYF+*Kg}jk_Cx5Jk@t~2bSs9ymWLYbV=rbMtBtOJ0tG^2fb8jgq#Nb zX?Fq3aw?N!CQV8KID}lLbAmT>T)&V0Ie2g3{{V?}{{RYV0p+^X94u~7+`N8_i5wJW)9ZTuzR-A-$} z`!$YR+e@OV4gly6psz*!nEV@i@c#heTv`sOB^NB)q*7)aP7wOD*@1RA9`b24{yILEChd;-IaRaRq=LFf6^Yt2<{e)#}a zv3Fxl-0+Y1DW{6tLH(ZfJ(4oB*tQSjU(O$hZw&r4)$U2%=3O&q)aJi;zu>mN7Bt@p z{>|>f;Y5>J7}pKod9T4FnB2`R_j#DH&3aT~bVAnDon%ggkIIroQ=9?qPzNRQNu_7c z@f>?sbWEP-*r{>0YM(L3;X~X<{4JXIs%(736`5{n#E?r7Jy4Fd z^3TQF5o6<-<;Fb5C-twU4c5gwx*j^$&!4m%vfKEB&q6+CuPVB^>tVJz7+CG(;P%B? zu{$qfV!y{#EnIm+o@&jsgi5mv0;FLNUwhF*IbY?o5Xb2Q}Obt09SV`RATO#cA!=W}9% z{+hAPSC{)?TW#Iq7ClTn3P~<)gB|w^P|2!65Zt+JdJ1R`yWoYDoPQTIJKMY4t?70M z+d0L4*LW7-9|vjfr#XNV?_Y-h01o_74UdnsyOx4LMatmg+*j?-!uzpx@aIRm>Autv zpRGf>F{pH*#GMnGvu=!}2423kQqi}tCAjvj1YwMV4k~VPmf&WL45!kp!8M7o=+b2OI`4v zhx|u#6~?!0WN5$Dm5{DG^%NAJbISh!Xm0`dW5)8gjXo%NmUuLqA^!jpyI5tKGv;F; zA1#LCkWb@Xm+YYri7fsWMxG=R?R%2if1e>{@;`)LqHhn!4~{i^Yt2A58SQO^N(ZLg&{Ai3l5PWP-C3dP{IWD8Jz2?+>-- z#GeA|0xio8sa@FVlv@1QJ|BC7<`{wcSL)yV94n<6bT1BTsG0I`xyPsj`d8)syS~ua zJdV}q(7TDeU6F|X6D2b7V0z+_d?98=UL2mCE29mS*xCH)8Mwe+{i~rw(Kd6f;T=37 z9#%8$Q9pxhCm-?dG3#A0Xz`t+=x8k(jiG-kPAJhkoSeP_u|F$9F;&~){*-9(fND35Pb6{ltmg#Lq$Or^PT?QuLnDLfEsw2~a0;~#}U z)~f5up0&^H70#b;?%3k>4K8@?+0+xXRVyppCLhxk+TZC+@fC--u18St&Y-hyf;Go{ zH%itJvCTJka)tf8ZdzUA2hi0PbX6dFR-B$0)JP$d1dcOMKZfsA;GoZ@YpNGUQZpoQ zps+O_<~)P?)`Xf3&;&GR@~D5YZd0sepG;QjRwSnWYaRz5T4o*lAFgWKX}3$jL)X%f z>DOJ<{!}`Qc8OS-hatLl%_x##{9j+1q&8O>Iad6sO_ZakBA2v%!qFpJo21}B>rNVl z;q!yB|Vm}Ir z)}4Vw=rl{WSa-7?In8#S3B89<)S$JL7Y`z+8TS?ET7AT@L{Zxr714MrU0pL=w7gn9{@9U1LkwtdW6cf!vK=oZHXBOAwD3|G(o9hjx+hCF)LsQ%EO5o|sv z>NZj7EB=pb3aAA4$NvCcxC>nXhw~I|E7%(2RA$-RPNGQDyts2}!*)i~_}4i0#pK+p zlbrOff%Mtt`F!H5+!5BAnnY+mUU{uyLQA1js+urPp5$TEgY8kbhvQ}5h-;@hZUA2| zr+QV>Q2d~09jmG|-L^PqV~d+jc!>)cs;svVla>6dyqipf{n&Y`cOD#uLAa) z_SfNG{2WF51!%tyv~LV+nn#PGyR)%Ku2vl#okIbGf;x)wF?4E8Ss*_CyNXsk5;>|f zD~<|rUn6PX@L!MkCyuazqrUNsMZdk_sZaE+%{TrFH~R+bzzc60YcXX10L#vcA`$tD z^1iExUuByoq4cu=t-@q`(y0Xe!n_94{tAQp7wVripTwE-=`ERG=4+tRzi$uOYgh`_ zpA)UdMnG~0^`z)kT?$U;uU_eP+KfJ2#e##?O;k?{StIRgZEEyZkGDxkn# zm4~JH%fff;KE>j#I!yG%+JB97Hc`cR2iUJ5n?Li;Wi-@ex)E~qvWAbXNMj>B^c0cl zx|9eQTNB3w5m`Pa*6(ziR&}zFum_McTBO+)Bk@0pwEqAO+O@^>!ytU19!+_!ukjN9 z0K^w<4YF;IoC>MpJB@3^H%}zilVF|((@oGkF{m}+cD0dkr}zzY(}tqipPt|FT`wKA z@54yEI`W;yKAXDoYx0)N6|;;mBjvBqkN7U%jDEr6t3L=bkk0`m6Yu{3*RRgVwH8Ri z1P24sy-HC!R*1Yf>a>nX83XmIZK<=4K+n>kH_`?@zLnK$nQnblH4liAw%bnvuwG7N zop}9fQ6GxrTxv6mM*iwyu}GS=;TcP(Us4!~8u5=3PRw)3CpGKKZO;ZNUty2oVW|)= z{vhfQ1N<>6{{V$~SM3Gis|^nB2`n|Mo8Z{YmkKjprq?RCEcoZvyz}Fu!E>jqir|$z zb6$O09*rFjGqrQH0)Dk(H&r>udZ%K`w+Ego&it>+e+<_Pwa-B|;(NElT|!%L3dXU@ zz+iMZt;f{vSRL0c$2~V3SC{xx##*JOl(rVuk>ekHvCUq6e^|S0%XZ7^5Y@#TwN|HT zb$GclGh-h0O5TP67xAt|t|yhS_p+aT)a5aSAf4Xr&1j;mbJ-ix_`6IFR^D+b z$3!(&v(3ovXj`BTLF-T1p_dsVExFa2Gcc-oJVx3ThozWllID{Fa`j?#eI$c00kHS z0D_}w-?n$dNd6OOUkvT^yGZVqZ|zv`U8HOg#?m+k74gbBjf85v1svC0rAWg3z#g3|sU(YtDsJkupJ!rlqnuRH?IUOh9r&kY4pu+P zovvDTDz&SF_XpPPJ&r&6)#*b^fs=7j>%}p&g>D2u{uQE%V&+Ghp9bNGXfi#PqLaaZ z4ZEG~< z!W?7)^y^W}pumhnAvo{FCjoOK%uj&N-dM-dn0yH{H~PLkMS5Fl7Nx#bDo1=$Hiiey zkIs{(#+p3NPXSBGSorL5QGdb(cz7^w2imi+<<<0o@3T5@S|IpQq)=9jWdp-OD#4uKmsBT_NkcBE|tkT^e6 zT|;TgK2Z3F+K>C4Bdn|B3 z>r|kLz0NvK5@aW1ei^7=Nt7;FWRAkSH`5W{C08GMGcN6?KBYjcod&|^j4H~b?vY9)xBvko@z>@x>r#9l zvuyctfIghndHxHM35EXv5-L51kxE(~V7J$>{{XAInSPQHQYO2tCJcJL+WjtAe_HRQ z_&afure!1_?$mO88N4GRRX;_k_8vtkXmPQ4yTvg70IBgNqMz>H$Ul=+m_8wR_7j+V zZ>=HhQ&pH_@_OjM;ZEwxMiqD+#WDU8YH@PO`qNl=^+IwpVEDE1YTbidd{Xf&UY=iw zE=Tf`wCq1JxuJd^Yf9TBwo&iT_*MO0 zlidpENvD3;pS0$gkFxx7@p#8f_>SQJ05+{P{ky+v-z#H$Y4Jmj`?a|n`PUSm54BC? z`5F9bU-(zrk6*G8?^E?!UB3hz$6*ic+xuC-`L_7w;)L|ush{e#ucd#%NB;nA9U4WJ z*W=%c8*lJE`VfCO>(68GRoqLsM0az?6y)%Q$j6xmKPo<}4uvTq4~<_Nz9)XrI_mhB z;%|uV^&7NZqFcEGFdt9{rE#%o^B1)?edOl5_VCJpK(`?MDhWJ16!8%`_MyVScGG-IxdUykoCrP3RVn;^tz81E z3dPS=HOj7_$111urkmG|gKs(aHPb0Jd8(33-nrKGYoZoiN?bQwHY*Rs=4o{E=3L1B zftxhX?Kfl2e;S8Uy!%bDiJPysYnFFsR27bT-I!;deJaW`p2ngO`A@=c9|1Y?C?O<evA!%)PKQ0^%lPU znzXr@!)=Z=-JUu%bK)Y}GO4ld5FDOIIjcz`s{lGyCZXcb3uqS4bFO%&OS6pzU8d9{ zTz&-Ck^D&i0D_AC%0CKs{{X^CZTGHC-8ZL{Z z!g3>RKX`0<$W*t}k}{+4HRAp-_}}qg$JZw3#hy3RCVkl@ zF62S^2Cgzm*sv$lo@iN|^={Sb(x*?CV}elRc1Y8gSJWrSlT^Bpp1x(nHh(iw1_1QW zzG|`(4+fuzU@OiQj>$zzTQn-jTuiZ#yV45aL!x)`KFb?sF8mP#80^uDqFi9#DP+rfw&%m zkQ|H_#(PucKrq^=7z|kZ(*?LE8#+|O)nl1u}lLSpiGPgaQI!lS5l%U~%;oY}=Bci2nc(Xj~`ASAp+IpeYMC zLzWyLdQY-Pju(?xOrB$H5rK{~N0+;xT>WWTmYJT`N=VuVC%p?`&72JWHF(BJ1mlVi zmIeazO(B`-Xa`f)p7znl%8YT+uN92(!1~i9v1vZxq~ZlNxAQ^DlaBP(w}h_Jrw6_& z+QBSbbMHsln;9pHo=_?Mme`G(kFO0wTB|lybM>on*@2uD=e55p+y*A(Mn0sacHh1;9~pFv7O?dd~{8f_I$o`rA_ zW8740q=}zsMHQhOVnLnM#zsuCD>2VwOO=n9p(c$QoQwnXsNY2m_mmM@W;S7$$jvv& zMI$P^1RpoX<8>sJ4;vC4!n?PT@6+k|MqHP@)fC56vsJRjDg z5@)`BsRl^S593)jVv!>NeSZT<@-7sLK;RB(3xYYtLoH|ofOrG)sUdtGKpCi1p7j3! zF^AoyWIM9pxG{~P>Hh#~E!2$F6?Su$r2w!bj+E7qn=}$-ZFfVepUb%oF$1}>ezhQzpOEzKX*eb(u5|t`_^#yixpIbpvU2Az-+uqG{xxc3zpiwl)*r@c%b zBAGZI*`x&n0vD}PiUEtl;nZ_c7!R41#ye)MI-nmjU{gY=I0vsoidJAoAaHjye~xJl zkdTriWAD^efSXfmzD|15qq<}v9QxH;Kr4l4feLvYyHH4FEI`TZ4ND_n44}`iujx_j z5A!OIeAL+i63pswiakiAOiLyi54~6x?T}Se3}%LZEa0HesHTt+3uOl!jQY@FR2VB( zVox$NfPJZ+SrP@6dX6dC6o^QHh+JUfnjYCo6B%5NneS2?aKyO-1JatTc9c`LrpW_q zp$pqc0{q;O&lKg|7{*S0>O(G5J2w&26xTB>c`Cg6ih40~F*lvNagtA^G_DRAPAOA8 zSaVJRz`+?F=8jP2WJ-+_7Giex1XQuPVe$+N^u;fj8}BAKsgWR(Jdh4MlTy+eE0>i< z@H+OQR#^t^od=~#(8f94_)}HMVz}TMRw#pZFm`mO3z$hN(YN7KZ8D4?{HY9Oc)+715a*h6 zpwG&q@T63X5>7MjXtE8)Eb+&cIUj{P_b>ckhG=NF9Ix`t9zj;zj1L{@i&4tJ9jd@n z{Nw`%^Qh7iTjm`0+G$KjIW3$}3geB;1wK>H(wNeP7$BcQMIcZcav(mm8DwBn$}|iW zx2Yk3$A3zRqB1t`G!yluA*1B*52ZvT`HmKpfO&#L$mj8-cpNV{`Wk#w<+k%nB8-EA zbLmh8M7EVl>4Vr)Wb;vSvF`Q3tB#;!3d1ztv$l8#rg;?Xz^1n@S8G7Zan~Z8vKPih zOEskXw-C7YG>Z&jcNVGTL%Tc#GO5XGaFhd-VtsQ=7yt}V0^`A~-vTY!4{P$TR| zPR4+w4Zw7uGoFT(tOLL~!2GFpw@Qz5gMtbCDrF3LWB62nn_Jfu^i~6g@69NWJoESp zltu^x2Q<x!}< zj31e5P$3Juk8DsTK^2)9W?nl~h6P`lBgya0O&gZz4m(qvoPt5=+JquCjGllGr95zQ z=kTK_yoDh9nqA#+N99(4Qo}t5_`uWrJ$mrIsGx!3WlcKT3Hl}178 zO=jcFX&GBnpG`6Dl%qy}5lR052pm@j&XCBgdIhew4L&s+c69Hyt;c8(0;*0useN5H zsx?)lj(0@xMWvDRor%v?b6eKdc5t$Yky8h^6=kACUBym&;-<7f$bWaoV_U-yO)U_e zX+7BVEC2wLj>4t{6#rd20VQzxDy#6-L+2#(xi^^IGbSv z=e<`jIbG^N9_E=en_KSZpGu%LnmELMU~|d!=}Q|%t>wI&dWy_R*Z`~e^G^Fl4gmaf zRDjUI3|s?&(-g0171CxxanymuRgUq>?PvXJWV{HY&L`ZCm3x6U+gNs>O!1CQOaxd5 zU;`Zis(ySghHUkz5u-mhnnL|~qR0t~L%VPTKIWYorf}_?jNl%XOi3A&A;H_)q!Olb zgX=&RCXzoS2EoT;QHd7;iR0R$VjX}#tw!QNe|Cc&;+jo?XkA9xT=eNuM*Dzf1MoFf zEb>-BbLzD-I8%oKKpbtMs|DF1UNgTw$F)RRGlTh3rU_u~InA5;nj9Y=h}VtOToZBvS7O1Je}Moa6mqKT%K? zDuf2f0+tC7u5+F`3ILtmF}R+z$B!p)AC*VD%*Y%H960$zjgPGsOb4Rlu0|+rtl*QM zV@zf~UKRPL!3ij2FVDReNOm2!z6auISfic*2lJ+|DmM}M3W!Pq`G!27t|>i)P7x$K zSb^)sG{F#K<+lT<1EoRsqM(r*XFUPwN|yx>UZ=ezQrvsRF8pI76g*N!2_BUyGRYZi zVY>|RNU@nzF<=fk8KUf>u=4_kQ_gB{Fzf{Sb)W@`LPBTiDH&0|ZG=#e2p&RyQgPho zn%D<&sNLL=PK#zYDWrI__gjwiXaF5{@}oYr6Nypsv4RhCQ=&((!GucaY^pPR~P&O0?V(`n)V00T>M@+<^L z>(q9tiE%0ou1P+mPzAMVae{*$)aD2mBw&3iymuD>{mhSQuq0U%1asU_10{?)jiWsE z^r*$SJeKS2Q^O>YoJ^c{AW*Fnu>>$a>p&IS)jEJP>J3S;*MPrDl1qG#m-*CB8x9#c z9StU|0!SEuN$F9rbX>CnG3!v=-iQ$SnHUw#YrZl|bs{;i0-W+iPcs9hn*QDpRNSW> zvsw2(A+?C*pFOM2wQnDpizG}>JRQ}Cd*aStKu}LUm1%A8 zI42b}R%%9D@~%QkVm3T;%{^_&;eVB07Kx03PfK+ez!aLe6yvav;MB`%6l315MIUTa z$|{WQ2k@k#1!!%~a2BUqP5S-(s00Mqh%{(J8AcASx2x5=`2soz_#QeDD)})Hx z11zTfGI;}S+=x9dL6ZHJwMCmACRkRk(UY_F+DixQi$Y9FaTml ze$`0<0fHpjrrn7>#aW8l0K2h(J!#p)a-F>?<6u&JrU%s3q&CHYBW{aQ0@1e6zzp}O zC08I6I6aL8&d;2Zan}`TAqze1Fv;b~^B+az)rg(}8*h9Ze4u*PaDfcQNZ>_d)$z?e z07d}dpHo(Vwr+ofDa}U9G{m!>y=9$E{dX36F^R?@&HK#AooL?SUjWV+3vKQhB~; z<&R3K8tyq*@DEyJ=bWjc4LUUuD#eR-!D*LL1|)hmGGAS;;9 zG>WblGP?c-qFbHsgo%J1aw+UpjzJ^Zm=hrY4n}Hql^E?OfITXKMhpmHgVLpm_8e!Q zb5#&6Jb?*a-%4)aTXM1IwMQho6XgJ(r7SEKJh=qb0V5WS;Rw&INfgos2PY@5y+s^? zNj^Xi9qA;x*hBlCAm6TylaX|DnpHhl3 z%N5T~C`ppxG95`B&sxW~oA;Zh`B$(U)rgiSK5TMek<@pm&_g7o^fRU$HAdm;rl2!z6KmjMx zq7Whi!a{lAQ`%AGDA9Q%r(^C;m##Z7N?(&9A)nwM^Zx+P~Uq+sCHeXJXika`NU zDJdSsZhc6mBFGe(STN5}D6q!cXEcM_hLmT$vCYap_2oSpN01PQWWqF>F3R zI&!>@3X@S?ym$KON>ru#MV#B{$=bLGbSizfcJF}W- zVtFE`low*9dQ@gSbw8zI<*8>w_98hXQ-!ye(epycU9q^-o=^>t+4VGNW;Im+!xDai zo}l9ZV?F6Y4Dd5jEG$PnQUQP}jAEV=HyI+FmaOLizO^GpQ{@LV(gGtAD8XFu=}(6Q z5U6Ts)E=u&BWD|V^s2BC`D|EVVEfbjjBk zjoHmAvZ%;v6-LkqsfDt#?`|{DVt^SQ`3@1k6H`MRfq;H#&_}Rgln!cr%d>LwN3~J` zcFv#?z|SKbnt7T~H!6a0(3*)Rm4-=B2dy&Z?npprtlDh%Mi>=9T~(RaasWMfrrb!_ z0I}n)Dym#uxWkQu`=nEtp*%O@Dw6_e?&Wi^fwXWkDSXfhsrBU5f4Xq5_|wMipoaJ6 zs0Yn2K&yeCO)(1fCnMg28IzU=uRPR#TH|P#fCXH30%T0LnZ`b~W+I4kqn^g9Tewx@ zk)KMiW#j}y$9z(d6NkvbKT}c2?yrS>iep>=Hs>VL#-WaKeqMQ`tOSu@67I(s>6%7# zVgiiQCuWgOIHk0bOLQvz=+?kUeAoF%c`0ABn~@L z*b2+$x!uk`ojoR1a;NddK3uT`9CJ(xq02DMT6Y4(WeSbpdlOMyLS`9`J`cSF#f+)W zdsCM1!U6%?ietFcTw!1Er<)^jCivyX-&0ZVd7pSd2YzZsWk~mo{VJj>3cJ54}W_VnW_tanqWaq*si* zObdEY0!weTLZQjWT12+3-J6RhFT5~rsCj8OC3bB4ejsSJc;apIz7+}|Vh z?LdhmxtZL4ryXi(B~Ws`O+b+{jfaqXij2rNlqY7<{70ak$9C#$W;moJ$sqI{ zg+&a&u~ljnodFKux2**i3}PT8P_3UqNbFC}SMaKjeQ_K>$0s@Iijvk@Pwz~Dy+r^` z^7RL;Ao83LSDINRECVQ95#E|5iytcB&;Yi+S>A&`jX%s{aLOs3V}<_!Rh;xWr^4b$ z{{VaIPT(tSkcJuKsG*rcans(TSru4?&*xG{63#Z~6eNIPU|%`N^%SnMkCy`n)|~d{ z5w#mWq|-~b@0f}jC6RraZN=0Bj`Zk^u@YV}pK6nTlOFu;9q@Woer!*Y9O9tzjEY_C&A2Ey z{b(t>39*&16j8PuktpZaG_5kQ07OCWOp-Tr&Otv~28V1^`D5S##!W$S9Ih~`4?U`Q zqJO&7mzjbhLLNK%)JoO^it${N!7GVyKf7E;t2pE^=hnNQ7auw&WBJ#f>T)=g$BdAD zE0&uky0wC>r(^(V2XMxXKE)Vv@;+o zG4g|x?@#%H@C`{MZijO&J*bLLF@lsIjTQiefIfE$YE*=_32)M$_BCQK06qAtF~b_J zNzGC~M39g&pbC%zN6w&nntKj#s6gq`l_7FC2l1)`CPBY;NdB~<1E*?!1Fq6DnqiJ* z9dJFm(Pn^ra~-&G$)QvNSc6Oo3Hh__Qfy!`yd2el{{Ssms{@=<%+_vnFIi{FVA92Y(^h(k!nN}F~G^kO&d6?vLk6N{`0x?oK z>U}D5^6lJ=IPX;?A>U zm?$~)_Ni5({{U9`2d6aL?Es({$6-k%t-LW&NC^yJ4eD}p#YT>eDUL+J13Aq|SUQ5F zj(gNZ9mlyIm2ud82bO_2W&lweB$0v!;0Fexk}y|u0y*@gis52yig`S9Ohn-a-Briq zPcg<9E`M5yXIBRq2fq}O1(0AT1G%b*)yX$UpWUu0;wap2XTTjzR!KI-B9WB(5HU?z z8Cz(ZaP+5p2(<9T2{93mT=l485ydfbGm(sDql$a8Az4C&KX=xqV;aZ47{P4w?@6`; zoJLOA!Te8JCs~p%0VID2sG*TxE`IiZDwIq?!xNHt=7iW?j)o)%3mg;Jp7kLd3?6gW zH5}p291sBHifELo?q>RPPT)@?q~H=WidNV_P{aXHE!f7uf}T9%^C0?BbOe&c6~rbt zkdI7LQ6)s$T(BOdn-j=mXPk4I8c0U`ZILJq{`J+6MV#V8=e0sBZ2Kz*TYq>zaMM?I`mJ!5xJI7(_|(I$(l1 zr+(?&6`Xwtrrv<=tf%TRNgwV`^#GIWKocw?Jj`Sa2k_#jibXK_(k2TIfK_uKAaBP$ z)RM}|pd%a}v;ebQgnhgaMJy{cjlElpcBqvn4ZbzW?g*)bj#M!PSarn!Oi`I|cs{fh zF+@q^Z5ssMg2zSF@x*Q9v190pJcB%W){^_}R<3XTnO zR7E2&$j6gGn2cccKD8W#XC!e^vveTWE6}6Aia6X3dHPhPLL8RQ;Yk{Ke{p*Er;O+4 zBbwIsGYyKzBbrsWP(j5@93E)hQV5k>AdjYL^D$h7l3uF6TCoCu*~(yT>p1Y6IiT7hld+dFes zL!fPrK=z=56OG4c#wk`ekmR==sby%}?vKWpC3a$|kxx)8GnGJqaZ!ivgC5i%elt%J zJ_uFk*A+`(BQiNZHfnTqVURJ8T4H1_OkGVqe69-&dsQvKKlA)g^c^~yWT1t@dUee=$(X1BoO;s$c7MLFfIUq}jgh)>^7P`P5UE14W73{_h28$> zGyxlk<9x@Q9ze}X7)UTMdkT(d@avWYo}hN4gd2B(>}uA)Ot69T5sY=DVGEIr=QyV+ zEgk-Qgv(S1{mf$`lk~G_eJn?}_@<^m_7z3?Drg+I_TxX{=^pZv9Md~`5 zkQdH{^0CMm_ran$SnyYj(-zzoB@~aLsUrb$G1I+OfSx(PQ@`*Sq{)*dal!9Vw7ZyW zKU~zd;w3}!jQW~a0g7b|RO1BpsUi-~hTs#_Qzez7`NEUitu>vBWct-DhD&=OQnBvn zJ5w#*CqFRgDzS=iOS5DXz|B4dZTP_*=t{yT0#!TPX&K`kzO?ZqL=gV~&Y0Oo+7z9~ zo+=q6b0!!r&?=L#6G#?REQ`Q61XODo1P#L{(xOT37Ev1GZh0Ll+-4cnu_&Y-M(${C z;0tU@Wn||(liGu7RYMt01uN3-kSnYCpU{Y>b#sNHcr`v-U$gWAx8K#|ziDW?=+l7`R$mEKuxbKi>DL2#f9#dFW}46iUPA-RbQ>3kmZi0f#=6rgdq8MZ;(Kx>JfUJBV(bsZud4 zM5=ti>y(xz1#=HLT&ouj4RY{6f&PQ`qp3$N#%-fWl?axX8wq$^Q z_khH7rX-U`F7!}$U{n%CACb5*4U7(HY~_*^+i**B@|^Xc3FKD;bMc;)7nsnWm5yoc zdSn4g?Z>Sm=0@Hz#Q;wfvdp6dbm&D&(7J#e#OBhu=phou~^k=20`Waop2!9`JXNGf_u*xcPENjC4M<<0kLAcpkJCiDis3{D(BlNu-Ik z%%y)0DS~gC89a8Wu#+K94JHJK$$XWZqkD9!vu{ZVknYEEPe^>G`i14DpV4Ruv5iOs|w)vWm{rC4trBr zZy}k9C%q?@;1>jEy**eENL-o_iBc%S;idrMoC1T#N^EhRyVwl&pt(5XrC0+1&4N#` zsi9N~cN_|iFeGk8J)m>KPy(`*;T!QZvBWXDKwAQvutbHVf6lDgJZLvc20eXf0wzG< z6pI{mq)Z6{P9mKvDrY$7*47oE0Dr=9*hBH_5lq zRF5QE04FA_1=zcH@5t?nSf_V{#=s1EQpT-tHmV$S>sA)l+BDkiGwDDS&P02cBc6tz z-^rQr{M*dd#&JJ)f)`g9BZQM?JoYGG$ zmhgsW#^&H+nG#BphLJb|tDqBk2&fcrK&50%28HlX9cg5LF>+8~cc{FOqkou3IRF}v zCqetbrAZ&&sO}^^I|_zLh5&-BafDOf>jNtUe z08Xk11;`%Q=}_F;#M60%XSmHeJ;i~P9C`|o?Mr^}NyqnSxDcheEJ-*R;-y09q-@6> zNu;=rYhbb=89g@+DPxl1(-MK7P$&V7bs~`y52Z~QY-O7`=}eJ)##Y=1VhE@1RfMDc z@8QJ&JMB`loS_N_KJ=sslx|#7TFju2JqQD-E5%DXVfVrVD*?ehDP4gSwx|@H-zoR1 zzCuR7BN8*-q-KgT(ClBEJ*knzSxkGu#V{xNxC0|2ikd5!_w2~r2SZUKI8c&&*x+KG z(-5H%Z~*Qo8g?dG(6-fl5_!R-y1bIvgebu0u0=-oAfQk{Be|iKULsf$!>Ppsy~P(V zC(Ic1?kQrsX|o#w8`KId5;%~_y_hiQdr*k345`=BoyTHS-?U}ef$dB)Fmtq?v^;DS zmU27iH5{IBj53_y^NMM^g_$^LqyRC&9rH^VgMI{=U^||*RF}^3nG}f~u|X=%1OEVa zs0|3DcKf{Z(w5`QW>>e7Qy+OT)~duLDVXHP<5EdI%v&Wy4bMA`11BpPA&x=z?NQI1 z102UAwII1Nxj4pYtsI57M zN~~GN3HGKdqYOCf=}nSRA`ld*^vzTkMMpX7K&)#SE!8}wT!K5)C;@V;GCNgEb2QJ9 zDBImlPR|?%cbMFIng%{bNFODBl{6D0jl+ZORoYnY0Cr4{#+NI*F_J$D7!SOi*M(p^ z(%Z{zDr}v(>_u0c$XjzMU^|LpD3I@qAom8NKrUTF6mgeXaz3?%saU1THq{wEwXGRo zn`J^l6<&B$w%|CZl0bQLG*MQffJTr7+qe&4 zDl*QCg?y7m6o3@YS9`b96iGbFyCZDpsiKM)1VzIB_3}?_Q`O8zFN_n%G*Lp_2c^K3 zj?Mt}rv+viUdNL~6<|t$E(tu;idzH)B8n&gF_J!R^s0&m3mPb(Phn$%vVz764I2Gz3oI+>jeBj&V*{;zTEI4u1A1qL>eXR$z94$8$>P@g5Hzf{G{s z$$J|7td(Znm6vpb<;Pkmpa;hpkxLxNLHSj&fl)QwHYir{imAswlu=V4BWWSJwlZ6> zE7TfT%eoAveN7ZpfiujKFpUTbqnc{URItg~2NY3FI{_2Gj^1|Rk~s#LvO7o>P>g<5 zQC0-dq{dC`2GP)FrH)9pvpSsr00^Rr0BBez$auy<>S-Ayj1|EO2NY321adwwj4?vb zEN27(>53?zcVwn;>yl0>3psM2W}jzttu0~uaO%!x2k2i?UiH$*~9 zk+@Ms08P0iPEd30NFsy)PDePvqKW`z%!?k~ub-)>#fKn(D58KQ`!aHK^%R0NL;IoD ziYNkO3KBP*=hmsq5E2#R8KQ~+77r}$M?Xrip=3?W4n63ifEsdu`DcOD3XkoNA0c?} zMHB$3aI+u0!1t$+(LVH2!ySbbQsqEQmypar#t)$s$ze^|Vg`MvqNS?=9mHEk6@xB0 V#b;mGMRgM@#gcl1MHP&I|JiB&4HW Date: Mon, 6 Aug 2018 20:52:43 -0400 Subject: [PATCH 06/47] move plotting to RoMEPlotting --- examples/wheeled/racecar/racecarUtils.jl | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index 5f5cc95cc..1678e3815 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -166,16 +166,6 @@ end -function plotPose2Vels(fgl::FactorGraph, sym::Symbol; coord=nothing) - X = getVertKDE(fgl, sym) - px = plotKDE(X, dims=[4], title="Velx") - coord != nothing ? (px.coord = coord) : nothing - py = plotKDE(X, dims=[5], title="Vely") - coord != nothing ? (py.coord = coord) : nothing - hstack(px, py) -end - - # From a7ea7abb0e3181c67ab39fdde7196bf48d910d93 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 7 Aug 2018 11:34:00 -0400 Subject: [PATCH 07/47] wip --- .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 157 +++++++--------- .../Img2AprilTagSLAM_local_randomDA.jl | 173 ++++++++++++++++++ .../racecar/apriltag_transforms_image.jl | 29 +-- examples/wheeled/racecar/cameraUtils.jl | 17 ++ .../wheeled/racecar/ensureTagOrientation.jl | 86 +++++++++ examples/wheeled/racecar/racecarUtils.jl | 146 ++++++--------- .../wheeled/racecar/visualizationUtils.jl | 127 +++++++++++++ 7 files changed, 522 insertions(+), 213 deletions(-) create mode 100644 examples/wheeled/racecar/Img2AprilTagSLAM_local_randomDA.jl create mode 100644 examples/wheeled/racecar/ensureTagOrientation.jl create mode 100644 examples/wheeled/racecar/visualizationUtils.jl diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index 1aef48026..c45b16378 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -9,11 +9,14 @@ using RoMEPlotting, Gadfly using AprilTags using Images, ImageView, ImageDraw using CoordinateTransformations, Rotations, StaticArrays +using MeshCat +using GeometryTypes const KDE = KernelDensityEstimate const IIF = IncrementalInference const TU = TransformUtils +# temporarily using Python / OpenCV for solvePnP using PyCall @pyimport numpy as np @@ -21,8 +24,8 @@ using PyCall include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) - include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cameraUtils.jl")) +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","visualizationUtils.jl")) cfg = loadConfig() @@ -39,20 +42,9 @@ Rx = RotX(-pi/2) Rz = RotZ(-pi/2) bTc= LinearMap(Rz) ∘ LinearMap(Rx) -datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" -# tag_bag[0] - +datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" imgfolder = "images" -# camcount = readdlm(datafolder*"$(imgfolder)/cam-count.csv",',') -camlookup = Dict{Int, String}() -count = -1 -for i in 175:5:370 - count += 1 - camlookup[count] = "camera_image$(i).jpeg" - # camlookup[camcount[i,1]] = strip(camcount[i,2]) -end - # Figure export folder @@ -62,45 +54,14 @@ mkdir(imgdir) mkdir(imgdir*"/tags") -# AprilTag detector -detector = AprilTagDetector() - -# extract tags from images -IMGS = [] -TAGS = [] -psid = 1 -for psid in 0:(length(camlookup)-1) - img = load(datafolder*"$(imgfolder)/$(camlookup[psid])") - tags = detector(img) - push!(TAGS, deepcopy(tags)) - push!(IMGS, deepcopy(img)) - foreach(tag->drawTagBox!(IMGS[psid+1],tag, width = 5, drawReticle = false), tags) - save(imgdir*"/tags/img_$(psid).jpg", IMGS[psid+1]) -end -# psid = 1 -# img = load(datafolder*"$(imgfolder)/$(camlookup[psid])") - -# free the detector memory -freeDetector!(detector) - - +# process images +camlookup = prepCamLookup(175:5:370) +IMGS, TAGS = detectTagsViaCamLookup(camlookup, datafolder*imgfolder, imgdir) # IMGS[1] # TAGS[1] -tvec = Translation(0.0,0,0) -q = Quat(1.0,0,0,0) -# package tag detections for all keyframes in a tag_bag -tag_bag = Dict{Int, Any}() -for psid in 0:(length(TAGS)-1) - tag_det = Dict{Int, Any}() - for tag in TAGS[psid+1] - q, tvec = getAprilTagTransform(tag, camK, k1, k2, tagsize) - cTt = tvec ∘ CoordinateTransformations.LinearMap(q) - tag_det[tag.id] = buildtagdict(cTt, q, tvec, tagsize, bTc) - end - tag_bag[psid] = tag_det -end -tag_bag[0] -TAGS[1] + +tag_bag = prepTagBag(TAGS) +# tag_bag[0] # save the tag bag file for future use @@ -109,18 +70,18 @@ TAGS[1] # Factor graph construction -N = 75 +N = 100 fg = initfg() psid = 0 pssym = Symbol("x$psid") # first pose with zero prior -addNode!(fg, pssym, Pose2) +addNode!(fg, pssym, DynPose2(ut=0)) # addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)))) addFactor!(fg, [pssym], DynPose2VelocityPrior(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)), - MvNormal(zeros(2),diagm([0.01;0.01].^2)))) + MvNormal(zeros(2),diagm([0.3;0.01].^2)))) -addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2) +addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2, fcttype=DynPose2Pose2) # writeGraphPdf(fg) @@ -128,69 +89,79 @@ addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2) tree = wipeBuildNewTree!(fg, drawpdf=true) inferOverTreeR!(fg,tree, N=N) -plotKDE(fg, :l1, dims=[3]) +# plotKDE(fg, :l1, dims=[3]) # ls(fg) # # val = getVal(fg, :l11) -drawPosesLandms(fg, spscale=0.25) +# drawPosesLandms(fg, spscale=0.25) # @async run(`evince bt.pdf`) prev_psid = 0 # add other positions maxlen = (length(tag_bag)-1) -for psid in [5;9;13;17] #17:4:21 #maxlen - addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=DynPose2) +# psid = 5 +for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen + @show psid + addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=VelPose2VelPose2, fcttype=DynPose2Pose2) # writeGraphPdf(fg) - if psid % 1 == 0 || psid == maxlen + if psid % 3 == 0 || psid == maxlen tree = wipeBuildNewTree!(fg, drawpdf=true) inferOverTree!(fg,tree, N=N) - + end + ensureAllInitialized!(fg) pl = drawPosesLandms(fg, spscale=0.1, drawhist=false)#, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); - Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),30cm, 25cm),pl) + Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),15cm, 10cm),pl) pl = drawPosesLandms(fg, spscale=0.1)#, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); - Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),30cm, 25cm),pl) - end + Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),15cm, 10cm),pl) + pl = plotPose2Vels(fg, Symbol("x$(psid)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0)) + Gadfly.draw(PNG(joinpath(imgdir,"vels_x$(psid).png"),15cm, 10cm),pl) prev_psid = psid end -IIF.savejld(fg, file=imgdir*"/racecar_fg_$(currdirtime).jld") - -ls(fg, :l7) - -stuff = IIF.localProduct(fg, :l7) -plotKDE(stuff[2], levels=3, dims=[3]) - -tag_bag[28][7][:tRYc] -tag_bag[31][7][:tRYc] - - -plotKDE(fg, :l1, dims=[3]) - -getfnctype(getVert(fg, :x28l7f1, nt=:fnc)).z.μ -getfnctype(getVert(fg, :x31l7f1, nt=:fnc)).z.μ - -ls(fg, :l17) - -getfnctype(getVert(fg, :x28l17f1, nt=:fnc)).z.μ -getfnctype(getVert(fg, :x30l17f1, nt=:fnc)).z.μ +IIF.savejld(fg, file=imgdir*"/racecar_fg_$(currdirtime).jld") -# fg, = IncrementalInference.loadjld(file="30jul18_9AM.jld") -# addFactor!(fg, [:l5;:l16], Point2Point2Range(Normal(0.5,1.0))) -# ls(fg,:l5) -fid = open("results.csv","w") -for sym in [ls(fg)[1]...;ls(fg)[2]...] - p = getVertKDE(fg, sym) - val = string(KDE.getKDEMax(p)) - println(fid, "$sym, $(val[2:(end-1)])") -end -close(fid) +# +# +# ls(fg, :l7) +# +# stuff = IIF.localProduct(fg, :l7) +# plotKDE(stuff[2], levels=3, dims=[3]) +# +# tag_bag[28][7][:tRYc] +# tag_bag[31][7][:tRYc] +# +# +# plotKDE(fg, :l1, dims=[3]) +# +# getfnctype(getVert(fg, :x28l7f1, nt=:fnc)).z.μ +# getfnctype(getVert(fg, :x31l7f1, nt=:fnc)).z.μ +# +# ls(fg, :l17) +# +# getfnctype(getVert(fg, :x28l17f1, nt=:fnc)).z.μ +# getfnctype(getVert(fg, :x30l17f1, nt=:fnc)).z.μ +# +# +# # fg, = IncrementalInference.loadjld(file="30jul18_9AM.jld") +# # addFactor!(fg, [:l5;:l16], Point2Point2Range(Normal(0.5,1.0))) +# # ls(fg,:l5) +# +# +# +# fid = open("results.csv","w") +# for sym in [ls(fg)[1]...;ls(fg)[2]...] +# p = getVertKDE(fg, sym) +# val = string(KDE.getKDEMax(p)) +# println(fid, "$sym, $(val[2:(end-1)])") +# end +# close(fid) diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local_randomDA.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local_randomDA.jl new file mode 100644 index 000000000..e3fa5564a --- /dev/null +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local_randomDA.jl @@ -0,0 +1,173 @@ + # Local compute version + +# add more julia processes +nprocs() < 5 ? addprocs(5-nprocs()) : nothing + +using Caesar, RoME, Distributions +using YAML, JLD, HDF5 +using RoMEPlotting, Gadfly +using AprilTags +using Images, ImageView, ImageDraw +using CoordinateTransformations, Rotations, StaticArrays +using MeshCat +using GeometryTypes + +const KDE = KernelDensityEstimate +const IIF = IncrementalInference +const TU = TransformUtils + +# temporarily using Python / OpenCV for solvePnP +using PyCall + +@pyimport numpy as np +@pyimport cv2 + + +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cameraUtils.jl")) +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","visualizationUtils.jl")) + + +cfg = loadConfig() + +cw, ch = cfg[:intrinsics][:cx], cfg[:intrinsics][:cy] +fx = fy = cfg[:intrinsics][:fx] +camK = [fx 0 cw; 0 fy ch; 0 0 1] +tagsize = 0.172 +# k1,k2 = cfg[:intrinsics][:k1], cfg[:intrinsics][:k2] # makes it worse +k1, k2 = 0.0, 0.0 + +# tag extrinsic rotation +Rx = RotX(-pi/2) +Rz = RotZ(-pi/2) +bTc= LinearMap(Rz) ∘ LinearMap(Rx) + + +datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" +imgfolder = "images" + + +# Figure export folder +currdirtime = now() +imgdir = joinpath(ENV["HOME"], "Pictures", "racecarimgs", "$(currdirtime)") +mkdir(imgdir) +mkdir(imgdir*"/tags") + + +# process images +camlookup = prepCamLookup(175:5:370) +IMGS, TAGS = detectTagsViaCamLookup(camlookup, datafolder*imgfolder, imgdir) +# IMGS[1] +# TAGS[1] + +tag_bag = prepTagBag(TAGS) +# tag_bag[0] + + +# save the tag bag file for future use +@save imgdir*"/tag_det_per_pose.jld" tag_bag + + + +# Factor graph construction +N = 100 +fg = initfg() + +psid = 0 +pssym = Symbol("x$psid") +# first pose with zero prior +addNode!(fg, pssym, DynPose2(ut=0)) +# addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)))) +addFactor!(fg, [pssym], DynPose2VelocityPrior(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)), + MvNormal(zeros(2),diagm([0.3;0.01].^2)))) + +addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2, fcttype=DynPose2Pose2) + +# writeGraphPdf(fg) + +# quick solve as sanity check +tree = wipeBuildNewTree!(fg, drawpdf=true) +inferOverTreeR!(fg,tree, N=N) + +# plotKDE(fg, :l1, dims=[3]) + +# ls(fg) +# +# val = getVal(fg, :l11) + +# drawPosesLandms(fg, spscale=0.25) + +# @async run(`evince bt.pdf`) + +prev_psid = 0 +# add other positions +maxlen = (length(tag_bag)-1) +# psid = 5 +for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen + @show psid + addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=VelPose2VelPose2, fcttype=DynPose2Pose2, DAerrors=0.2) + # writeGraphPdf(fg) + if psid % 3 == 0 || psid == maxlen + tree = wipeBuildNewTree!(fg, drawpdf=true) + inferOverTree!(fg,tree, N=N) + end + ensureAllInitialized!(fg) + pl = drawPosesLandms(fg, spscale=0.1, drawhist=false)#, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),15cm, 10cm),pl) + pl = drawPosesLandms(fg, spscale=0.1)#, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),15cm, 10cm),pl) + pl = plotPose2Vels(fg, Symbol("x$(psid)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0)) + Gadfly.draw(PNG(joinpath(imgdir,"vels_x$(psid).png"),15cm, 10cm),pl) + prev_psid = psid +end + + +IIF.savejld(fg, file=imgdir*"/racecar_fg_$(currdirtime).jld") + + + + +# +# +# ls(fg, :l7) +# +# stuff = IIF.localProduct(fg, :l7) +# plotKDE(stuff[2], levels=3, dims=[3]) +# +# tag_bag[28][7][:tRYc] +# tag_bag[31][7][:tRYc] +# +# +# plotKDE(fg, :l1, dims=[3]) +# +# getfnctype(getVert(fg, :x28l7f1, nt=:fnc)).z.μ +# getfnctype(getVert(fg, :x31l7f1, nt=:fnc)).z.μ +# +# ls(fg, :l17) +# +# getfnctype(getVert(fg, :x28l17f1, nt=:fnc)).z.μ +# getfnctype(getVert(fg, :x30l17f1, nt=:fnc)).z.μ +# +# +# # fg, = IncrementalInference.loadjld(file="30jul18_9AM.jld") +# # addFactor!(fg, [:l5;:l16], Point2Point2Range(Normal(0.5,1.0))) +# # ls(fg,:l5) +# +# +# +# fid = open("results.csv","w") +# for sym in [ls(fg)[1]...;ls(fg)[2]...] +# p = getVertKDE(fg, sym) +# val = string(KDE.getKDEMax(p)) +# println(fid, "$sym, $(val[2:(end-1)])") +# end +# close(fid) + + + + +0 + + + +# diff --git a/examples/wheeled/racecar/apriltag_transforms_image.jl b/examples/wheeled/racecar/apriltag_transforms_image.jl index 1ecb24654..6d3bdc966 100644 --- a/examples/wheeled/racecar/apriltag_transforms_image.jl +++ b/examples/wheeled/racecar/apriltag_transforms_image.jl @@ -14,6 +14,8 @@ using PyCall include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cameraUtils.jl")) include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","visualizationUtils.jl")) + cfg = loadConfig() @@ -59,32 +61,6 @@ setobject!(vis["home"], Triad(1.0)) -function drawTagDetection(vis::Visualizer, tagname, Q, T, bTc, bP2t; posename=:test) - # draw tag triad - setobject!(vis[currtag], Triad(0.2)) - settransform!(vis[currtag], bTt) - - # draw ray to tag - v = vis[posename][:lines][tagname] - geometry = PointCloud( - Point.([bTt.translation[1];0], - [bTt.translation[2];0], - [0.0;0]) - ) - setobject!(v, LineSegments(geometry, LineBasicMaterial())) - settransform!(v, Translation(0.0,0,0)) - - # draw orientation tail for tag - v = vis[posename][:orient][tagname] - geometry = PointCloud( - Point.([0.1;0], - [0.0;0], - [0.0;0]) - ) - setobject!(v, LineSegments(geometry, LineBasicMaterial())) - settransform!(v, bP2t) # - nothing -end tidx = 1 @@ -111,7 +87,6 @@ bP2t = getTagPP2(bTt) drawTagDetection(vis, currtag, Q, T, bTc, bP2t, posename=:x5) - end diff --git a/examples/wheeled/racecar/cameraUtils.jl b/examples/wheeled/racecar/cameraUtils.jl index a92a4dbf1..0915363e4 100644 --- a/examples/wheeled/racecar/cameraUtils.jl +++ b/examples/wheeled/racecar/cameraUtils.jl @@ -119,6 +119,23 @@ function getAprilTagTransform(tag::AprilTag; end +function prepTagBag(TAGS) + tvec = Translation(0.0,0,0) + q = Quat(1.0,0,0,0) + # package tag detections for all keyframes in a tag_bag + tag_bag = Dict{Int, Any}() + for psid in 0:(length(TAGS)-1) + tag_det = Dict{Int, Any}() + for tag in TAGS[psid+1] + q, tvec = getAprilTagTransform(tag, camK, k1, k2, tagsize) + cTt = tvec ∘ CoordinateTransformations.LinearMap(q) + tag_det[tag.id] = buildtagdict(cTt, q, tvec, tagsize, bTc) + end + tag_bag[psid] = tag_det + end + return tag_bag +end + # get Pose2Pose2 tag orientation transform function getTagPP2(bTt) @show bTt diff --git a/examples/wheeled/racecar/ensureTagOrientation.jl b/examples/wheeled/racecar/ensureTagOrientation.jl new file mode 100644 index 000000000..8496475c6 --- /dev/null +++ b/examples/wheeled/racecar/ensureTagOrientation.jl @@ -0,0 +1,86 @@ +# script to confirm all AprilTag sightings are consistent + +using YAML, JLD, HDF5 +using AprilTags +using Images, ImageView, ImageDraw +using CoordinateTransformations, Rotations, StaticArrays +using MeshCat +using GeometryTypes + +# temporarily using Python / OpenCV for solvePnP +using PyCall +@pyimport numpy as np +@pyimport cv2 + +# load utility function for racecar +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl")) +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cameraUtils.jl")) +include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","visualizationUtils.jl")) + +# load camera calibration +cfg = loadConfig() +cw, ch = cfg[:intrinsics][:cx], cfg[:intrinsics][:cy] +fx = fy = cfg[:intrinsics][:fx] +camK = [fx 0 cw; 0 fy ch; 0 0 1] +tagsize = 0.172 +# k1,k2 = cfg[:intrinsics][:k1], cfg[:intrinsics][:k2] # makes it worse +k1, k2 = 0.0, 0.0 + +# tag extrinsic rotation +Rx = RotX(-pi/2) +Rz = RotZ(-pi/2) +bTc= LinearMap(Rz) ∘ LinearMap(Rx) + +# image loading directory +datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" +imgfolder = "images" + +# Figure export folder +currdirtime = now() +imgdir = joinpath(ENV["HOME"], "Pictures", "testimgs", "$(currdirtime)") +mkdir(imgdir) +mkdir(imgdir*"/tags") + +# process images +camlookup = prepCamLookup(175:5:370) +IMGS, TAGS = detectTagsViaCamLookup(camlookup, datafolder*imgfolder, imgdir) +tag_bag = prepTagBag(TAGS) + +# save the tag bag file for future use +@save imgdir*"/tag_det_per_pose.jld" tag_bag + + + + +# 3D visualization +vis = Visualizer() +open(vis) + +setobject!(vis["home"], Triad(1.0)) + + + + +currtag = "" +bTt = LinearMap(Translation(0.0,0,0) ∘ Quat(1.0,0,0,0)) + + +psid = 5 +tags = tag_bag[psid] + +# tidx = 1 +# tag = tags[tidx] +for (tidx, tag) in tags + +currtag = "tag$(tidx)" +Q = Quat(tag[:quat]...) +cTt = (Translation(tag[:pos]...) ∘ LinearMap(Q)) +bTt = bTc ∘ cTt +drawTagDetection(vis, currtag, Q, tag[:pos], bTc, tag[:bP2t], posename=Symbol("x$psid")) + +end + + + + +# diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index 1678e3815..5262f97a6 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -24,15 +24,16 @@ end # add AprilTag sightings from this pose -function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1, lmtype=Point2 ) - currtags = ls(fg)[2] +function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1, lmtype=Point2, fcttype=Pose2Pose2, DAerrors=0.0 ) + @show currtags = ls(fg)[2] for lmid in keys(posetags) @show lmsym = Symbol("l$lmid") if !(lmsym in currtags) + info("adding node $lmsym") addNode!(fg, lmsym, lmtype) end ppbr = nothing - if lmtype == Point2 + if lmtype == RoME.Point2 ppbr = Pose2Point2BearingRange(Normal(posetags[lmid][:bearing][1],bnoise), Normal(posetags[lmid][:range][1],rnoise)) elseif lmtype == Pose2 @@ -43,18 +44,29 @@ function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1, lmtype=Point # -posetags[lmid][:pos][1]; # posetags[lmid][:tRYc]], # diagm([0.1;0.1;0.05].^2)) ) - ppbr = Pose2Pose2( + ppbr = fcttype( MvNormal([dx; dy; dth], - diagm([0.3;0.1;0.1].^2)) ) + diagm([0.1;0.1;0.01].^2)) ) + end + if rand() > DAerrors + # regular single hypothesis + addFactor!(fg, [pssym; lmsym], ppbr, autoinit=false) + else + # artificial errors to data association occur + info("Forcing bad data association with $lmsym") + xx,ll = ls(fg) + @show ll2 = setdiff(ll, [lmsym]) + @show daidx = round(Int, (length(ll2)-1)*rand()+1) + @show rda = ll2[daidx] + addFactor!(fg, [pssym; lmsym; rda], ppbr, autoinit=false, multihypo=[1.0;0.5;0.5]) end - addFactor!(fg, [pssym; lmsym], ppbr, autoinit=false) end nothing end -function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odotype=Pose2Pose2) +function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odotype=Pose2Pose2, fcttype=Pose2Pose2, DAerrors=0.0) prev_pssym = Symbol("x$(prev_psid)") new_pssym = Symbol("x$(new_psid)") # first pose with zero prior @@ -62,103 +74,53 @@ function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odot addNode!(fg, new_pssym, Pose2) addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.4;0.1;0.4].^2)))) elseif odotype == VelPose2VelPose2 - addNode!(fg, new_pssym, DynPose2(ut=round(Int, 1000_000*(new_psid/6.0)))) - addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.4;0.1;0.4].^2)), - MvNormal(zeros(2),diagm([0.1;0.01].^2)))) + addNode!(fg, new_pssym, DynPose2(ut=round(Int, 200_000*(new_psid)))) + addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.3;0.07;0.1].^2)), + MvNormal(zeros(2),diagm([0.2;0.05].^2)))) end - addApriltags!(fg, new_pssym, pose_tag_bag, lmtype=lmtype) + addApriltags!(fg, new_pssym, pose_tag_bag, lmtype=lmtype, fcttype=fcttype, DAerrors=DAerrors) new_pssym end -function drawThickLine!(image, startpoint, endpoint, colour, thickness) - (row, col) = size(image) - x1 = startpoint.x - y1 = startpoint.y - x2 = endpoint.x - y2 = endpoint.y - - draw!(image, LineSegment(x1,y1,x2,y2), colour) - - if x1 != x2 && (y2-y1)/(x2-x1) < 1 - for tn = 1:thickness - i = tn ÷ 2 - - x1mi = x1-i - x1pi = x1+i - y1mi = y1-i - y1pi = y1+i - x2mi = x2-i - x2pi = x2+i - y2mi = y2-i - y2pi = y2+i - #clip to protect bounds - (x1mi < 1) && (x1mi = 1) - (y1mi < 1) && (y1mi = 1) - (x1pi > col) && (x1pi = col) - (y1pi > row) && (y1pi = row) - (x2mi < 1) && (x2mi = 1) - (y2mi < 1) && (y2mi = 1) - (x2pi > col) && (x2pi = col) - (y2pi > row) && (y2pi = row) - - iseven(tn) && draw!(image, LineSegment(x1,y1mi,x2,y2mi), colour) - isodd(tn) && draw!(image, LineSegment(x1,y1pi,x2,y2pi), colour) - end - else - for tn = 1:thickness - i = tn ÷ 2 - - x1mi = x1-i - x1pi = x1+i - y1mi = y1-i - y1pi = y1+i - x2mi = x2-i - x2pi = x2+i - y2mi = y2-i - y2pi = y2+i - #clip to protect bounds - (x1mi < 1) && (x1mi = 1) - (y1mi < 1) && (y1mi = 1) - (x1pi > col) && (x1pi = col) - (y1pi > row) && (y1pi = row) - (x2mi < 1) && (x2mi = 1) - (y2mi < 1) && (y2mi = 1) - (x2pi > col) && (x2pi = col) - (y2pi > row) && (y2pi = row) - - iseven(tn) && draw!(image, LineSegment(x1mi,y1,x2mi,y2), colour) - isodd(tn) && draw!(image, LineSegment(x1pi,y1,x2pi,y2), colour) - end - end +function prepCamLookup(imgseq; filenamelead="camera_image") + # camcount = readdlm(datafolder*"$(imgfolder)/cam-count.csv",',') + camlookup = Dict{Int, String}() + count = -1 + for i in imgseq + count += 1 + camlookup[count] = "$(filenamelead)$(i).jpeg" + # camlookup[camcount[i,1]] = strip(camcount[i,2]) + end + return camlookup end +function detectTagsViaCamLookup(camlookup, imgfolder, imgsavedir) + # AprilTag detector + detector = AprilTagDetector() + # extract tags from images + IMGS = [] + TAGS = [] + psid = 1 + for psid in 0:(length(camlookup)-1) + img = load("$(imgfolder)/$(camlookup[psid])") + tags = detector(img) + push!(TAGS, deepcopy(tags)) + push!(IMGS, deepcopy(img)) + foreach(tag->drawTagBox!(IMGS[psid+1],tag, width = 5, drawReticle = false), tags) + save(imgsavedir*"/tags/img_$(psid).jpg", IMGS[psid+1]) + end + # psid = 1 + # img = load(datafolder*"$(imgfolder)/$(camlookup[psid])") + # free the detector memory + freeDetector!(detector) -function drawTagLine!(imgl, tag_detection) - # naive - # ch, cw = round(Int, height/2), round(Int, width/2) - cw, ch = 330.4173, 196.32587 # from ZED driver config - focal = 340.97913 - imheight = 376 - - tagsize = 0.172 - - bearing0 = tag_detection[:bearing][1] - - # convert bearing to position on image - tag_u_coord = cw + focal*tan(-bearing0) - tag_u_coord = round(Int, tag_u_coord) - - c1 = Point(tag_u_coord, 1) - c2 = Point(tag_u_coord, imheight) - drawThickLine!(imgl, c1, c2, RGB{N0f8}(0.8,0.0,0.4), 10) - - nothing + return IMGS, TAGS end @@ -166,6 +128,4 @@ end - - # diff --git a/examples/wheeled/racecar/visualizationUtils.jl b/examples/wheeled/racecar/visualizationUtils.jl new file mode 100644 index 000000000..ea712ffa4 --- /dev/null +++ b/examples/wheeled/racecar/visualizationUtils.jl @@ -0,0 +1,127 @@ + + + +function drawTagDetection(vis::Visualizer, tagname, Q, T, bTc, bP2t; posename=:test) + # draw tag triad + setobject!(vis[currtag], Triad(0.2)) + settransform!(vis[currtag], bTt) + + # draw ray to tag + v = vis[posename][:lines][tagname] + geometry = PointCloud( + GeometryTypes.Point.([bTt.translation[1];0], + [bTt.translation[2];0], + [0.0;0]) + ) + setobject!(v, LineSegments(geometry, LineBasicMaterial())) + settransform!(v, Translation(0.0,0,0)) + + # draw orientation tail for tag + v = vis[posename][:orient][tagname] + geometry = PointCloud( + GeometryTypes.Point.([0.1;0], + [0.0;0], + [0.0;0]) + ) + setobject!(v, LineSegments(geometry, LineBasicMaterial())) + settransform!(v, bP2t) # + nothing +end + + + +function drawThickLine!(image, startpoint, endpoint, colour, thickness) + (row, col) = size(image) + x1 = startpoint.x + y1 = startpoint.y + x2 = endpoint.x + y2 = endpoint.y + + draw!(image, LineSegment(x1,y1,x2,y2), colour) + + if x1 != x2 && (y2-y1)/(x2-x1) < 1 + for tn = 1:thickness + i = tn ÷ 2 + + x1mi = x1-i + x1pi = x1+i + y1mi = y1-i + y1pi = y1+i + x2mi = x2-i + x2pi = x2+i + y2mi = y2-i + y2pi = y2+i + #clip to protect bounds + (x1mi < 1) && (x1mi = 1) + (y1mi < 1) && (y1mi = 1) + (x1pi > col) && (x1pi = col) + (y1pi > row) && (y1pi = row) + (x2mi < 1) && (x2mi = 1) + (y2mi < 1) && (y2mi = 1) + (x2pi > col) && (x2pi = col) + (y2pi > row) && (y2pi = row) + + iseven(tn) && draw!(image, LineSegment(x1,y1mi,x2,y2mi), colour) + isodd(tn) && draw!(image, LineSegment(x1,y1pi,x2,y2pi), colour) + end + else + for tn = 1:thickness + i = tn ÷ 2 + + x1mi = x1-i + x1pi = x1+i + y1mi = y1-i + y1pi = y1+i + x2mi = x2-i + x2pi = x2+i + y2mi = y2-i + y2pi = y2+i + #clip to protect bounds + (x1mi < 1) && (x1mi = 1) + (y1mi < 1) && (y1mi = 1) + (x1pi > col) && (x1pi = col) + (y1pi > row) && (y1pi = row) + (x2mi < 1) && (x2mi = 1) + (y2mi < 1) && (y2mi = 1) + (x2pi > col) && (x2pi = col) + (y2pi > row) && (y2pi = row) + + iseven(tn) && draw!(image, LineSegment(x1mi,y1,x2mi,y2), colour) + isodd(tn) && draw!(image, LineSegment(x1pi,y1,x2pi,y2), colour) + end + end +end + + + + + + +function drawTagLine!(imgl, tag_detection) + # naive + # ch, cw = round(Int, height/2), round(Int, width/2) + warn("Using hard-coded camera calibration parameters in drawTagLine!") + cw, ch = 330.4173, 196.32587 # from ZED driver config + focal = 340.97913 + imheight = 376 + + tagsize = 0.172 + + bearing0 = tag_detection[:bearing][1] + + # convert bearing to position on image + tag_u_coord = cw + focal*tan(-bearing0) + tag_u_coord = round(Int, tag_u_coord) + + c1 = Point(tag_u_coord, 1) + c2 = Point(tag_u_coord, imheight) + drawThickLine!(imgl, c1, c2, RGB{N0f8}(0.8,0.0,0.4), 10) + + nothing +end + + + + + +# From e3bb250c33ce6dde2902c391bb812cf69ffd1101 Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 8 Aug 2018 11:50:33 -0400 Subject: [PATCH 08/47] wip --- examples/basic/valuesfromjld.jl | 34 +++++++++++++++++++ .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 22 +++++++----- examples/wheeled/racecar/racecarUtils.jl | 4 +-- 3 files changed, 49 insertions(+), 11 deletions(-) create mode 100644 examples/basic/valuesfromjld.jl diff --git a/examples/basic/valuesfromjld.jl b/examples/basic/valuesfromjld.jl new file mode 100644 index 000000000..3d56ec247 --- /dev/null +++ b/examples/basic/valuesfromjld.jl @@ -0,0 +1,34 @@ +# extract estimates from jld file + +using Caesar, Distributions, RoME + +const KDE = KernelDensityEstimate + +datadir = "/home/dehann/Pictures/racecarimgs/2018-08-08T10:29:13.58/" +filename = datadir*"racecar_fg_2018-08-08T10:29:13.58.jld" + +fg, = loadjld(file=filename) + + +# using RoMEPlotting +# drawPosesLandms(fg, spscale=0.1) + +xx,ll = ls(fg) + + +fid = open(datadir*"results_x39.csv","w") +for x in xx + # @show x, + mx = KDE.getKDEMean(getVertKDE(fg, x)) + println(fid, "$x, $(mx[1]), $(mx[2]), $(mx[3]), $(mx[4]), $(mx[5])") +end +for x in ll + # @show x, + mx = KDE.getKDEMean(getVertKDE(fg, x)) + println(fid, "$x, $(mx[1]), $(mx[2])") +end +close(fid) + + + +# diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index c45b16378..c0d5b16b6 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -104,25 +104,29 @@ prev_psid = 0 maxlen = (length(tag_bag)-1) # psid = 5 for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen - @show psid + @show psym = Symbol("x$psid") addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=VelPose2VelPose2, fcttype=DynPose2Pose2) # writeGraphPdf(fg) if psid % 3 == 0 || psid == maxlen tree = wipeBuildNewTree!(fg, drawpdf=true) inferOverTree!(fg,tree, N=N) end - ensureAllInitialized!(fg) - pl = drawPosesLandms(fg, spscale=0.1, drawhist=false)#, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); - Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),15cm, 10cm),pl) - pl = drawPosesLandms(fg, spscale=0.1)#, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); - Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),15cm, 10cm),pl) - pl = plotPose2Vels(fg, Symbol("x$(psid)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0)) - Gadfly.draw(PNG(joinpath(imgdir,"vels_x$(psid).png"),15cm, 10cm),pl) + + # save factor graph for later testing and evaluation + IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym).jld") + ensureAllInitialized!(fg) + pl = drawPosesLandms(fg, spscale=0.1, drawhist=false,xmin=-1.0,xmax=5.0,ymin=-2.0,ymax=2.0)#, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"$(psym).png"),15cm, 10cm),pl) + pl = drawPosesLandms(fg, spscale=0.1,xmin=-1.0,xmax=5.0,ymin=-2.0,ymax=2.0)#, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); + Gadfly.draw(PNG(joinpath(imgdir,"hist_$(psym).png"),15cm, 10cm),pl) + pl = plotPose2Vels(fg, Symbol("$(psym)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0)) + Gadfly.draw(PNG(joinpath(imgdir,"vels_$(psym).png"),15cm, 10cm),pl) + + # prepare for next iteration prev_psid = psid end -IIF.savejld(fg, file=imgdir*"/racecar_fg_$(currdirtime).jld") diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index 5262f97a6..35c44a2e4 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -75,8 +75,8 @@ function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odot addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.4;0.1;0.4].^2)))) elseif odotype == VelPose2VelPose2 addNode!(fg, new_pssym, DynPose2(ut=round(Int, 200_000*(new_psid)))) - addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.3;0.07;0.1].^2)), - MvNormal(zeros(2),diagm([0.2;0.05].^2)))) + addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.4;0.07;0.1].^2)), + MvNormal(zeros(2),diagm([0.28;0.05].^2)))) end addApriltags!(fg, new_pssym, pose_tag_bag, lmtype=lmtype, fcttype=fcttype, DAerrors=DAerrors) From caa587a800eb25ff081f51137bf312b603087851 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 14 Aug 2018 00:03:30 -0400 Subject: [PATCH 09/47] wip --- examples/basic/valuesfromjld.jl | 7 ++++--- examples/wheeled/racecar/Img2AprilTagSLAM_local.jl | 13 +++++++++---- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/examples/basic/valuesfromjld.jl b/examples/basic/valuesfromjld.jl index 3d56ec247..c29871236 100644 --- a/examples/basic/valuesfromjld.jl +++ b/examples/basic/valuesfromjld.jl @@ -4,8 +4,9 @@ using Caesar, Distributions, RoME const KDE = KernelDensityEstimate -datadir = "/home/dehann/Pictures/racecarimgs/2018-08-08T10:29:13.58/" -filename = datadir*"racecar_fg_2018-08-08T10:29:13.58.jld" +datetimestamp = "2018-08-09T00:04:36.373" +datadir = "/home/dehann/Pictures/racecarimgs/$(datetimestamp)/" +filename = datadir*"racecar_fg_x237.jld" fg, = loadjld(file=filename) @@ -16,7 +17,7 @@ fg, = loadjld(file=filename) xx,ll = ls(fg) -fid = open(datadir*"results_x39.csv","w") +fid = open(datadir*"results_$(datetimestamp).csv","w") for x in xx # @show x, mx = KDE.getKDEMean(getVertKDE(fg, x)) diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index c0d5b16b6..f2fe57551 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -43,7 +43,11 @@ Rz = RotZ(-pi/2) bTc= LinearMap(Rz) ∘ LinearMap(Rx) -datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" +# datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" +# datafolder = ENV["HOME"]*"/data/racecar/labrun2/" +# datafolder = ENV["HOME"]*"/data/racecar/labrun3/" +# datafolder = ENV["HOME"]*"/data/racecar/labrun5/" +datafolder = ENV["HOME"]*"/data/racecar/labrun6/" imgfolder = "images" @@ -55,7 +59,8 @@ mkdir(imgdir*"/tags") # process images -camlookup = prepCamLookup(175:5:370) +# camlookup = prepCamLookup(175:5:370) +camlookup = prepCamLookup(0:5:1795) IMGS, TAGS = detectTagsViaCamLookup(camlookup, datafolder*imgfolder, imgdir) # IMGS[1] # TAGS[1] @@ -115,9 +120,9 @@ for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen # save factor graph for later testing and evaluation IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym).jld") ensureAllInitialized!(fg) - pl = drawPosesLandms(fg, spscale=0.1, drawhist=false,xmin=-1.0,xmax=5.0,ymin=-2.0,ymax=2.0)#, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); + pl = drawPosesLandms(fg, spscale=0.1, drawhist=false)#, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); Gadfly.draw(PNG(joinpath(imgdir,"$(psym).png"),15cm, 10cm),pl) - pl = drawPosesLandms(fg, spscale=0.1,xmin=-1.0,xmax=5.0,ymin=-2.0,ymax=2.0)#, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); + pl = drawPosesLandms(fg, spscale=0.1)#, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); Gadfly.draw(PNG(joinpath(imgdir,"hist_$(psym).png"),15cm, 10cm),pl) pl = plotPose2Vels(fg, Symbol("$(psym)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0)) Gadfly.draw(PNG(joinpath(imgdir,"vels_$(psym).png"),15cm, 10cm),pl) From 7dac7fb8b794bba4f76f40f419b0095991716d62 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 19 Aug 2018 17:38:01 -0400 Subject: [PATCH 10/47] work in progress --- .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 30 +++++++++++++------ examples/wheeled/racecar/racecarUtils.jl | 2 +- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index f2fe57551..228cd6a2f 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -47,12 +47,14 @@ bTc= LinearMap(Rz) ∘ LinearMap(Rx) # datafolder = ENV["HOME"]*"/data/racecar/labrun2/" # datafolder = ENV["HOME"]*"/data/racecar/labrun3/" # datafolder = ENV["HOME"]*"/data/racecar/labrun5/" -datafolder = ENV["HOME"]*"/data/racecar/labrun6/" +# datafolder = ENV["HOME"]*"/data/racecar/labrun6/" +datafolder = ENV["HOME"]*"/data/racecar/labfull/" imgfolder = "images" # Figure export folder currdirtime = now() +# currdirtime = "2018-08-14T00:52:01.534" imgdir = joinpath(ENV["HOME"], "Pictures", "racecarimgs", "$(currdirtime)") mkdir(imgdir) mkdir(imgdir*"/tags") @@ -60,7 +62,7 @@ mkdir(imgdir*"/tags") # process images # camlookup = prepCamLookup(175:5:370) -camlookup = prepCamLookup(0:5:1795) +camlookup = prepCamLookup(0:5:1765) IMGS, TAGS = detectTagsViaCamLookup(camlookup, datafolder*imgfolder, imgdir) # IMGS[1] # TAGS[1] @@ -71,7 +73,7 @@ tag_bag = prepTagBag(TAGS) # save the tag bag file for future use @save imgdir*"/tag_det_per_pose.jld" tag_bag - +# @load imgdir*"/tag_det_per_pose.jld" tag_bag # Factor graph construction @@ -84,7 +86,7 @@ pssym = Symbol("x$psid") addNode!(fg, pssym, DynPose2(ut=0)) # addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)))) addFactor!(fg, [pssym], DynPose2VelocityPrior(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)), - MvNormal(zeros(2),diagm([0.3;0.01].^2)))) + MvNormal(zeros(2),diagm([0.05;0.05].^2)))) addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2, fcttype=DynPose2Pose2) @@ -112,7 +114,7 @@ for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen @show psym = Symbol("x$psid") addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=VelPose2VelPose2, fcttype=DynPose2Pose2) # writeGraphPdf(fg) - if psid % 3 == 0 || psid == maxlen + if psid % 30 == 0 || psid == maxlen tree = wipeBuildNewTree!(fg, drawpdf=true) inferOverTree!(fg,tree, N=N) end @@ -120,9 +122,9 @@ for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen # save factor graph for later testing and evaluation IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym).jld") ensureAllInitialized!(fg) - pl = drawPosesLandms(fg, spscale=0.1, drawhist=false)#, meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2); + pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) #,xmin=-3,xmax=6,ymin=-5,ymax=2); Gadfly.draw(PNG(joinpath(imgdir,"$(psym).png"),15cm, 10cm),pl) - pl = drawPosesLandms(fg, spscale=0.1)#, meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2); + pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); Gadfly.draw(PNG(joinpath(imgdir,"hist_$(psym).png"),15cm, 10cm),pl) pl = plotPose2Vels(fg, Symbol("$(psym)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0)) Gadfly.draw(PNG(joinpath(imgdir,"vels_$(psym).png"),15cm, 10cm),pl) @@ -131,12 +133,22 @@ for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen prev_psid = psid end +tree = wipeBuildNewTree!(fg, drawpdf=true) +# @async run(`evince bt.pdf`) +inferOverTree!(fg,tree, N=N) +inferOverTree!(fg,tree, N=N) +# save factor graph for later testing and evaluation +IIF.savejld(fg, file=imgdir*"/racecar_fg_final.jld") +pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) #,xmin=-3,xmax=6,ymin=-5,ymax=2); +Gadfly.draw(PNG(joinpath(imgdir,"final.png"),15cm, 10cm),pl) +pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); +Gadfly.draw(PNG(joinpath(imgdir,"hist_final.png"),15cm, 10cm),pl) - -# +0 +#0 # # ls(fg, :l7) # diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index 35c44a2e4..3c2ae66b9 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -76,7 +76,7 @@ function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odot elseif odotype == VelPose2VelPose2 addNode!(fg, new_pssym, DynPose2(ut=round(Int, 200_000*(new_psid)))) addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.4;0.07;0.1].^2)), - MvNormal(zeros(2),diagm([0.28;0.05].^2)))) + MvNormal(zeros(2),diagm([0.2;0.2].^2)))) end addApriltags!(fg, new_pssym, pose_tag_bag, lmtype=lmtype, fcttype=fcttype, DAerrors=DAerrors) From 9eb1dc60abbd0fa66e4c234f0e128c9aa06d7f9e Mon Sep 17 00:00:00 2001 From: GearsAD Date: Tue, 28 Aug 2018 22:42:37 -0500 Subject: [PATCH 11/47] Added user and robot to all calls --- src/Caesar.jl | 2 + src/cloudgraphs/CloudGraphIntegration.jl | 94 +++++++++++++++-------- src/cloudgraphs/ConvertGeneralSlaminDB.jl | 20 ++--- src/cloudgraphs/IterationStatistics.jl | 16 ++++ src/cloudgraphs/MultisessionUtils.jl | 34 +++++--- src/cloudgraphs/slamindb.jl | 28 ++++--- 6 files changed, 127 insertions(+), 67 deletions(-) create mode 100644 src/cloudgraphs/IterationStatistics.jl diff --git a/src/Caesar.jl b/src/Caesar.jl index 10b169e0e..632323863 100644 --- a/src/Caesar.jl +++ b/src/Caesar.jl @@ -129,6 +129,7 @@ export # webserver SolverStatus, CaesarConfig, + IterationStatistics, VisualizationConfig, # multisession utils @@ -156,6 +157,7 @@ include("config/CaesarConfig.jl") # using CloudGraphs include("cloudgraphs/SolverStatus.jl") +include("cloudgraphs/IterationStatistics.jl") include("cloudgraphs/CloudGraphIntegration.jl") # Work in progress code include("cloudgraphs/ConvertGeneralSlaminDB.jl") include("cloudgraphs/slamindb.jl") diff --git a/src/cloudgraphs/CloudGraphIntegration.jl b/src/cloudgraphs/CloudGraphIntegration.jl index 29e9dcacf..871590dc3 100644 --- a/src/cloudgraphs/CloudGraphIntegration.jl +++ b/src/cloudgraphs/CloudGraphIntegration.jl @@ -61,12 +61,14 @@ executeQuery(cg::CloudGraph, query::AS) where {AS <:AbstractString} = executeQue function getCloudVert( cg::CloudGraph, session::AbstractString, + robot::AbstractString, + user::AbstractString, vsym::Symbol; bigdata::Bool=false ) # warn("getCloudVert(cg, sess, sym) will be deprecated, use getCloudVert(cg, sess, sym=sym) instead.") # query = " and n.ready=$(ready) and n.label=$(vsym) " # query = reqbackendset ? query*" and n.backendset=$(backendset)" : query - query = "match (n:$(session)) where n.label='$(vsym)' return id(n)" + query = "match (n:$(session):$robot:$user) where n.label='$(vsym)' return id(n)" cph, = executeQuery(cg, query) # loadtx = transaction(cg.neo4j.connection) @@ -76,13 +78,15 @@ function getCloudVert( end function getCloudVert(cgl::CloudGraph, - session::AbstractString; + session::AbstractString, + robot::AbstractString, + user::AbstractString; exvid::VoidUnion{Int}=nothing, neoid::VoidUnion{Int}=nothing, sym::VoidUnion{Symbol}=nothing, bigdata=false ) # - query = "match (n:$(session)) " + query = "match (n:$(session):$robot:$user) " if sym != nothing query = query*" where n.label='$(sym)' " @@ -102,9 +106,9 @@ function getCloudVert(cgl::CloudGraph, end -function listAllVariables(cgl::CloudGraph, session::AbstractString) +function listAllVariables(cgl::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) # - query = "match (n:$(session)) where not (n:FACTOR) and exists(n.exVertexId) and n.ready=1 return n.label, n.exVertexId, id(n), labels(n)" + query = "match (n:$(session):$robot:$user) where not (n:FACTOR) and exists(n.exVertexId) and n.ready=1 return n.label, n.exVertexId, id(n), labels(n)" cph, = executeQuery(cgl.neo4j.connection, query) dd = Dict{Symbol, Tuple{Int, Int, Vector{Symbol}}}() @@ -129,7 +133,7 @@ List neighbors to node in cgl::CloudGraph by returning Dict{Sym}=(exvid, neoid, any of the three as input node identifier. Not specifying an identifier will result in all Variable nodes being returned. """ -function ls(cgl::CloudGraph, session::AbstractString; +function ls(cgl::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString; sym::VoidUnion{Symbol}=nothing, neoid::VoidUnion{Int64}=nothing, exvid::VoidUnion{Int64}=nothing ) @@ -137,9 +141,9 @@ function ls(cgl::CloudGraph, session::AbstractString; if sym == nothing && exvid == nothing && neoid == nothing # interrupt and just return all variable nodes - return listAllVariables(cgl, session) + return listAllVariables(cgl, session, robot, user) end - cv = getCloudVert(cgl, session, sym=sym, neoid=neoid, exvid=exvid, bigdata=false ) + cv = getCloudVert(cgl, session, robot, user, sym=sym, neoid=neoid, exvid=exvid, bigdata=false ) neis = get_neighbors(cgl, cv, needdata=false) dd = Dict{Symbol, Tuple{Int, Int, Vector{Symbol}}}() @@ -155,11 +159,12 @@ function getfnctype(cvl::CloudGraphs.CloudVertex) return getfnctype(vert) end -function initfg(;sessionname="NA",robotname="",cloudgraph=nothing) +function initfg(;sessionname="NA",robotname="",username="",cloudgraph=nothing) # fgl = RoME.initfg(sessionname=sessionname) fgl = IncrementalInference.emptyFactorGraph() fgl.sessionname = sessionname fgl.robotname = robotname + fgl.username = username fgl.cg = cloudgraph return fgl end @@ -419,12 +424,14 @@ function getAllExVertexNeoIDs(conn::Neo4j.Connection; backendset::Int=1, sessionname::AS="", robotname::AS="", + username::AS="", reqbackendset::Bool=true, reqready::Bool=true ) where {AS <: AbstractString} # sn = length(sessionname) > 0 ? ":"*sessionname : "" rn = length(robotname) > 0 ? ":"*robotname : "" - query = "match (n$(sn)$(rn)) where not n:SESSION and exists(n.exVertexId)" + un = length(username) > 0 ? ":"*username : "" + query = "match (n$(sn)$(rn)$(un)) where not n:SESSION and exists(n.exVertexId)" query = reqbackendset || reqready ? query*" and" : query query = reqready ? query*" n.ready=$(ready)" : query query = reqbackendset && reqready ? query*" and" : query @@ -580,6 +587,7 @@ function getLblExVertexNeoIDs( lbls::Vector{AS}; session::AS="", robot::AS="", + user::AS="", label::AS="", reqready::Bool=true, ready::Int=1, @@ -767,7 +775,7 @@ function subLocalGraphCopy!( # warn("subGraphCopy! is a work in progress") conn = fgl.cg.neo4j.connection - IDs = getLblExVertexNeoIDs(conn, string.(lbls), session=fgl.sessionname, robot=fgl.robotname, reqbackendset=reqbackendset, reqready=reqready, neighbors=neighbors ) + IDs = getLblExVertexNeoIDs(conn, string.(lbls), session=fgl.sessionname, robot=fgl.robotname, user=fg.username, reqbackendset=reqbackendset, reqready=reqready, neighbors=neighbors ) println("fullSubGraphCopy: $(length(IDs)) nodes in session $(fgl.sessionname) if reqbackendset=$reqbackendset and reqready=$reqready...") copyGraphNodesEdges!(fgl, IDs) nothing @@ -785,8 +793,8 @@ function fullLocalGraphCopy!( reqready::Bool=true ) # conn = fgl.cg.neo4j.connection - IDs = getAllExVertexNeoIDs(conn, sessionname=fgl.sessionname, robotname=fgl.robotname, reqbackendset=reqbackendset, reqready=reqready) - println("fullLocalGraphCopy: $(length(IDs)) nodes in robot=$(fgl.robotname), session $(fgl.sessionname) if reqbackendset=$reqbackendset and reqready=$reqready...") + IDs = getAllExVertexNeoIDs(conn, sessionname=fgl.sessionname, robotname=fgl.robotname, username=fgl.username, reqbackendset=reqbackendset, reqready=reqready) + println("fullLocalGraphCopy: $(length(IDs)) nodes in subgraph for user=$(fgl.username), robot=$(fgl.robotname), session=$(fgl.sessionname) if reqbackendset=$reqbackendset and reqready=$reqready...") copyGraphNodesEdges!(fgl, IDs) end @@ -797,11 +805,15 @@ Set all Neo4j nodes in this session ready = 1, warning function does not support """ function setDBAllReady!( conn::Neo4j.Connection, - sessionname::AS) where {AS <: AbstractString} + sessionname::AS, + robotname::AS, + username::AS) where {AS <: AbstractString} # warn("Obsolete setDBAllReady! function, see SynchronySDK for example ready function instead.") sn = length(sessionname) > 0 ? ":"*sessionname : "" - query = "match (n$(sn)) set n.ready=1" + rn = length(robotname) > 0 ? ":"*robotname : "" + un = length(username) > 0 ? ":"*username : "" + query = "match (n$(sn)$(rn)$(un)) set n.ready=1" cph, loadresult = executeQuery(conn, query) nothing end @@ -814,10 +826,14 @@ end function setBackendWorkingSet!( conn::Neo4j.Connection, - sessionname::AbstractString ) + sessionname::AbstractString, + robotname::AbstractString, + username::AbstractString ) # sn = length(sessionname) > 0 ? ":"*sessionname : "" - query = "match (n$(sn)) where not (n:NEWDATA) set n.backendset=1" + rn = length(robotname) > 0 ? ":"*robotname : "" + un = length(username) > 0 ? ":"*username : "" + query = "match (n$(sn)$(rn)$(un)) where not (n:NEWDATA) set n.backendset=1" cph, loadresult = executeQuery(conn, query) nothing @@ -888,6 +904,8 @@ function consoleaskuserfordb(;nparticles=false, drawdepth=false, clearslamindb=f !nparticles ? nothing : push!(need, "num particles") !drawdepth ? nothing : push!(need, "draw depth") !clearslamindb ? nothing : push!(need, "clearslamindb") + !user ? nothing : push!(need, "user") + !robot ? nothing : push!(need, "robot") !multisession ? nothing : push!(need, "multisession") !drawedges ? nothing : push!(need, "draw edges") @@ -899,6 +917,8 @@ function consoleaskuserfordb(;nparticles=false, drawdepth=false, clearslamindb=f n == "draw edges" ? print("[y]/n: ") : nothing n == "num particles" ? print("[100]: ") : nothing n == "clearslamindb" ? print("yes/[no]: ") : nothing + n == "user" ? print("[]: ") : nothing + n == "robot" ? print("[]: ") : nothing n == "multisession" ? print("comma separated list session names/[n]: ") : nothing str = readline(STDIN) addrdict[n] = str @@ -911,7 +931,7 @@ function consoleaskuserfordb(;nparticles=false, drawdepth=false, clearslamindb=f if drawdepth addrdict["draw depth"] = addrdict["draw depth"]=="" || addrdict["draw depth"]=="y" || addrdict["draw depth"]=="yes" ? "y" : "n" end - if drawdepth + if drawedges addrdict["draw edges"] = addrdict["draw edges"]=="" || addrdict["draw edges"]=="y" || addrdict["draw edges"]=="yes" ? "y" : "n" end if nparticles @@ -920,6 +940,12 @@ function consoleaskuserfordb(;nparticles=false, drawdepth=false, clearslamindb=f if clearslamindb addrdict["clearslamindb"] = addrdict["clearslamindb"]=="" || addrdict["clearslamindb"]=="n" || addrdict["clearslamindb"]=="no" ? "n" : addrdict["clearslamindb"] end + if user + addrdict["user"] = addrdict["user"]!="" ? addrdict["user"] : "" + end + if robot + addrdict["robot"] = addrdict["robot"]!="" ? addrdict["robot"] : "" + end if multisession addrdict["multisession"] = strip.(Vector{String}(split(addrdict["multisession"],','))) end @@ -1099,14 +1125,14 @@ function fetchsubgraph!(fgl::FactorGraph, end """ - getVertNeoIDs!(::CloudGraph, res::Dict{Symbol, Int}; session::AbstractString="NA") + getVertNeoIDs!(::CloudGraph, res::Dict{Symbol, Int}; session::AbstractString="NA", robot::AbstractString="NA", user::AbstractString="NA") Insert into and return dict `res` with Neo4j IDs of ExVertex labels as stored per session in Neo4j database. """ -function getVertNeoIDs!(cloudGraph::CloudGraph, res::Dict{Symbol, Int}; session::AbstractString="NA") +function getVertNeoIDs!(cloudGraph::CloudGraph, res::Dict{Symbol, Int}; session::AbstractString="NA", robot::AbstractString="NA", user::AbstractString="NA") loadtx = transaction(cloudGraph.neo4j.connection) syms = collect(keys(res)) - query = "match (n:$(session)) where " + query = "match (n:$(session):$robot:$user) where " for i in 1:length(syms) sym = syms[i] query =query*"n.label='$(sym)' " @@ -1143,26 +1169,26 @@ end """ - getfirstpose(cg::CloudGraph, session::AbstractString) + getfirstpose(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) Return Tuple{Symbol, Int} of first pose symbol and Neo4j node ID. """ -function getfirstpose(cg::CloudGraph, session::AbstractString) - query = "match (n:$(session):POSE) with n.label as nlbl, n.exVertexId as exvid, id(n) as neoid order by exvid asc limit 1 return nlbl, neoid" +function getfirstpose(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) + query = "match (n:$(session):$(robot):$(user):POSE) with n.label as nlbl, n.exVertexId as exvid, id(n) as neoid order by exvid asc limit 1 return nlbl, neoid" cph, = executeQuery(cg, query) # loadtx = transaction(cg.neo4j.connection) # cph = loadtx(query, submit=true) Symbol(cph.results[1]["data"][1]["row"][1]), cph.results[1]["data"][1]["row"][2] end -getFirstPose(cg::CloudGraph, session::AbstractString) = getfirstpose(cg, session) +getFirstPose(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) = getfirstpose(cg, session, robot, user) """ - getLastPose(cg::CloudGraph, session::AbstractString) + getLastPose(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) Return Tuple{Symbol, Int} of first pose symbol and Neo4j node ID. """ -function getLastPose(cg::CloudGraph, session::AbstractString) - query = "match (n:$(session):POSE) with n.label as nlbl, n.exVertexId as exvid, id(n) as neoid order by exvid desc limit 1 return nlbl, neoid" +function getLastPose(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) + query = "match (n:$(session):$robot:$user:POSE) with n.label as nlbl, n.exVertexId as exvid, id(n) as neoid order by exvid desc limit 1 return nlbl, neoid" cph, = executeQuery(cg, query) # loadtx = transaction(cg.neo4j.connection) # cph = loadtx(query, submit=true) @@ -1171,14 +1197,14 @@ end """ - insertrobotdatafirstpose!(cg::CloudGraph, session::AbstractString, robotdict::Dict) + insertrobotdatafirstpose!(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString, robotdict::Dict) Saves robotdict via JSON to first pose in a SESSION in the database. Used for storing general robot specific data in easily accessible manner. Can fetch later retrieve same dict with counterpart `fetchrobotdatafirstpose` function. """ -function insertrobotdatafirstpose!(cg::CloudGraph, session::AbstractString, robotdict::Dict) - vsym, neoid = getfirstpose(cg, session) +function insertrobotdatafirstpose!(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString, robotdict::Dict) + vsym, neoid = getfirstpose(cg, session, robot, user) cv = CloudGraphs.get_vertex(cg, neoid, true) appendvertbigdata!(cg, cv, "robot_description", json(robotdict).data ) end @@ -1199,14 +1225,14 @@ function tryunpackalltypes!(resp::Dict) end """ - fetchrobotdatafirstpose(cg::CloudGraph, session::AbstractString) + fetchrobotdatafirstpose(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) Return dict of JSON parsed "robot_description" field as was inserted by counterpart `insertrobotdatafirstpose!` function. Used for storing general robot specific data in easily accessible manner. """ -function fetchrobotdatafirstpose(cg::CloudGraph, session::AbstractString) - vsym, neoid = getfirstpose(cg, session) +function fetchrobotdatafirstpose(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) + vsym, neoid = getfirstpose(cg, session, robot, user) cv = CloudGraphs.get_vertex(cg, neoid, true) bde = Caesar.getBigDataElement(cv, "robot_description") resp = JSON.parse(String(take!(IOBuffer(bde.data)))) diff --git a/src/cloudgraphs/ConvertGeneralSlaminDB.jl b/src/cloudgraphs/ConvertGeneralSlaminDB.jl index 190d45f43..a4f7d0a4a 100644 --- a/src/cloudgraphs/ConvertGeneralSlaminDB.jl +++ b/src/cloudgraphs/ConvertGeneralSlaminDB.jl @@ -3,9 +3,9 @@ -function getmaxfactorid(conn, session::AbstractString) +function getmaxfactorid(conn, session::AbstractString, robot::AbstractString, user::AbstractString) loadtx = transaction(conn) - query = "match (n:$(session):FACTOR) + query = "match (n:$(session):$robot:$user:FACTOR) where not (n:NEWDATA) with id(n) as idn, n.exVertexId as nexvid order by nexvid desc limit 1 @@ -20,15 +20,15 @@ function getmaxfactorid(conn, session::AbstractString) end """ - getnewvertdict(conn, session) + getnewvertdict(conn, session::AbstractString, robot::AbstractString, user::AbstractString) Return a dictionary with frtend and mongo_keys json string information for :NEWDATA elements in Neo4j database. """ -function getnewvertdict(conn, session::AbstractString) +function getnewvertdict(conn, session::AbstractString, robot::AbstractString, user::AbstractString) loadtx = transaction(conn) - query = "match (n:$(session))-[:DEPENDENCE]-(f:NEWDATA:$(session):FACTOR) where n.ready=1 or f.ready=1 return distinct n, f" + query = "match (n:$(session):$robot:$user)-[:DEPENDENCE]-(f:NEWDATA:$(session):$robot:$user:FACTOR) where n.ready=1 or f.ready=1 return distinct n, f" cph = loadtx(query, submit=true) # loadresult = commit(loadtx) # @show cph.results[1] @@ -361,25 +361,25 @@ Convert vertices of session in Neo4j DB with Caesar.jl's required data elements in preparation for MM-iSAMCloudSolve process. """ function updatenewverts!(fgl::FactorGraph; N::Int=100) - sortedvd = getnewvertdict(fgl.cg.neo4j.connection, fgl.sessionname) + sortedvd = getnewvertdict(fgl.cg.neo4j.connection, fgl.sessionname, fgl.robotname, fgl.username) populatenewvariablenodes!(fgl, sortedvd, N=N) - maxfuid = getmaxfactorid(fgl.cg.neo4j.connection, fgl.sessionname) + maxfuid = getmaxfactorid(fgl.cg.neo4j.connection, fgl.sessionname, fgl.robotname, fgl.username) populatenewfactornodes!(fgl, sortedvd, maxfuid) nothing end """ - resetentireremotesession(conn, session) + resetentireremotesession(conn, session, robot, user) match (n:\$(session)) remove n.backendset, n.ready, n.data, n.bigData, n.label, n.packedType, n.exVertexId, n.shape, n.width set n :NEWDATA return n """ -function resetentireremotesession(conn, session::AbstractString; segment::AbstractString="") +function resetentireremotesession(conn, session::AbstractString, robot::AbstractString, user::AbstractString; segment::AbstractString="") loadtx = transaction(conn) - query = segment == "" ? "match (n:$(session)) " : "match (n:$(session):$(segment)) " + query = segment == "" ? "match (n:$(session):$robot:$user) " : "match (n:$(session):$robot:$user:$(segment)) " query = query*"where exists(n.frtend) remove n.backendset, n.ready, n.data, n.bigData, n.label, n.packedType, n.exVertexId, n.shape, n.width, n.MAP_est set n :NEWDATA" diff --git a/src/cloudgraphs/IterationStatistics.jl b/src/cloudgraphs/IterationStatistics.jl new file mode 100644 index 000000000..dbec63476 --- /dev/null +++ b/src/cloudgraphs/IterationStatistics.jl @@ -0,0 +1,16 @@ +using Base.Dates + +export IterationStatistics + +""" +A simple structure containing performance stats from the iteration. +""" +mutable struct IterationStatistics + startTimestamp::DateTime + endTimestamp::DateTime + numNodes::Int + result::String + additionalInfo::Dict{String, String} + IterationStatistics(startTimestamp, endTimestamp, numNodes, result, additionalInfo::Dict{String, String} = Dict{String, String}()) = new(startTimestamp, endTimestamp, numNodes, result, additionalInfo) + IterationStatistics() = new(Dates.now(), Dates.now(), 0, "N/A") +end diff --git a/src/cloudgraphs/MultisessionUtils.jl b/src/cloudgraphs/MultisessionUtils.jl index 83f8ff14b..ad6f7c5b7 100644 --- a/src/cloudgraphs/MultisessionUtils.jl +++ b/src/cloudgraphs/MultisessionUtils.jl @@ -2,11 +2,11 @@ -function multisessionquery(conn, session::T, multisessions::Vector{T}) where {T <: AbstractString} +function multisessionquery(conn, session::T, robot::T, user::T, multisessions::Vector{T}) where {T <: AbstractString} len = length(multisessions) loadtx = transaction(conn) # construct the query - query = "match (m:$(session):LANDMARK) "* + query = "match (m:$(session):$robot:$user:LANDMARK) "* "with m.label as mlb "* "order by mlb asc "* "match (n:LANDMARK) "* @@ -45,12 +45,14 @@ function parsemultisessionqueryresult!(lm2others::Dict{Symbol, Dict{Symbol, Int} end """ - getLandmOtherSessNeoIDs{T <: AbstractString}(::CloudGraph, session::T="", multisessions=Vector{T}()) + getLandmOtherSessNeoIDs{T <: AbstractString}(::CloudGraph, session::T="", robot::T="", user::T="", multisessions=Vector{T}()) Return dict of dict of Neo4j vertex IDs by session and landmark symbols. """ function getLandmOtherSessNeoIDs(cg::CloudGraph; session::T="", + robot::T="", + user::T="", multisessions::Vector{T}=String[] ) where {T <: AbstractString} # lm2others = Dict{Symbol, Dict{Symbol, Int}}() @@ -59,7 +61,7 @@ function getLandmOtherSessNeoIDs(cg::CloudGraph; len > 0 ? nothing : (return lm2others) if length(multisessions)==0 - cph = multisessionquery(cg.neo4j.connection, session, multisessions) + cph = multisessionquery(cg.neo4j.connection, session, robot, user, multisessions) parsemultisessionqueryresult!(lm2others, cph) else info("Ignoring multisession") @@ -126,17 +128,19 @@ key symbols used for graph exstraction. function getLocalSubGraphMultisession(cg::CloudGraph, lm2others; session::T="", + robot::T="", + user::T="", numneighbors::Int=0 ) where {T <: AbstractString} # res = Dict{Symbol, Int}() - sfg = Caesar.initfg(sessionname=session, cloudgraph=cg) + sfg = Caesar.initfg(sessionname=session, robotname=robot, username=user, cloudgraph=cg) if length(lm2others) > 0 for (sess,ms) in lm2others for (sym, neoid) in ms res[sym] = 0 end end - getVertNeoIDs!(cg, res, session=session) + getVertNeoIDs!(cg, res, session=session, robot=robot, user=user) fullcurrneolist = collect(values(res)) fetchsubgraph!(sfg, fullcurrneolist, numneighbors=numneighbors) # can set numneighbors=0 end @@ -151,7 +155,7 @@ Return Dict{Symbol, Int} of vertex symbol to Neo4j node ID of MULTISESSION const """ function findExistingMSConstraints(fgl::FactorGraph) loadtx = transaction(fgl.cg.neo4j.connection) - query = "match (n:$(fgl.sessionname):MULTISESSION) + query = "match (n:$(fgl.sessionname):$(fgl.robotname):$(fgl.username):MULTISESSION) return id(n), n.exVertexId, n.label" cph = loadtx(query, submit=true) @@ -195,26 +199,30 @@ end """ function rmInstMultisessionPriors!(cloudGraph::CloudGraph; session::T="NA", + robot::T="NA", + user::T="NA", multisessions::Vector{T}=String[] ) where {T <: AbstractString} # session!="NA" ? nothing : error("Please specify a valid session, currently = $(session)") + robot!="NA" ? nothing : error("Please specify a valid robot, currently = $(robot)") + user!="NA" ? nothing : error("Please specify a valid user, currently = $(user)") multisessionsl = Vector{String}(setdiff(multisessions, [session])) length(multisessionsl) > 0 ? nothing : (return nothing) # get the landmarks of interest from neighboring sessions lm2others = getLandmOtherSessNeoIDs(cloudGraph, - session=session,multisessions=multisessionsl) + session=session, robot=robot, user=user, multisessions=multisessionsl) # # grab local subgraph using NeoIDs sfg, lms = getLocalSubGraphMultisession(cloudGraph, lm2others, - session=session, numneighbors=1) + session=session, robot=robot, user=user,numneighbors=1) # # get dict(sym => neoid) of exiting multisession constraints which may be removed during this update exims = findExistingMSConstraints(sfg) # get highest factor exvid - mfn4jid = getmaxfactorid(cloudGraph.neo4j.connection, session) + mfn4jid = getmaxfactorid(cloudGraph.neo4j.connection, session, robot, user) println("Multisession constraints in $(session), from $(multisessionsl), on $(lms)") for sym in lms @@ -227,11 +235,13 @@ function rmInstMultisessionPriors!(cloudGraph::CloudGraph; end -function removeMultisessions!(cloudGraph::CloudGraph; session::AbstractString="NA") +function removeMultisessions!(cloudGraph::CloudGraph; session::AbstractString="NA", robot::AbstractString="NA", user::AbstractString="NA") session!="NA" ? nothing : error("Please specify a valid session, currently = $(session)") + robot!="NA" ? nothing : error("Please specify a valid robot, currently = $(robot)") + user!="NA" ? nothing : error("Please specify a valid user, currently = $(user)") loadtx = transaction(cloudGraph.neo4j.connection) - query = "match (n:$(session):MULTISESSION) + query = "match (n:$(session):$robot:$user:MULTISESSION) detach delete n return count(n)" cph = loadtx(query, submit=true) diff --git a/src/cloudgraphs/slamindb.jl b/src/cloudgraphs/slamindb.jl index ccd9ca89b..05badd26e 100644 --- a/src/cloudgraphs/slamindb.jl +++ b/src/cloudgraphs/slamindb.jl @@ -27,7 +27,8 @@ function runSlamInDbOnSession( sessionId::String, iterations::Int64, isRecursiveSolver::Bool, - solverStatus::SolverStatus )::Void + solverStatus::SolverStatus, + iterationCompleteCallback)::Void # N = caesarConfig.numParticles @@ -45,12 +46,13 @@ function runSlamInDbOnSession( while ((iterations > 0 || iterations == -1) && solverStatus.isAttached) iterations = iterations == -1 ? iterations : iterations-1 # stop at 0 or continue indefinitely if -1 + iterationStats = IterationStatistics() tic() solverStatus.iteration = itercount println("===================CONVERT===================") solverStatus.currentStep = "Prep_Convert" - fg = Caesar.initfg(sessionname=sessionId, robotname=robotId, cloudgraph=cloudGraph) + fg = Caesar.initfg(sessionname=sessionId, robotname=robotId, username=userId, cloudgraph=cloudGraph) updatenewverts!(fg, N=N) println() @@ -61,19 +63,20 @@ function runSlamInDbOnSession( println("================MULTI-SESSION================") solverStatus.currentStep = "Prep_MultiSession" - rmInstMultisessionPriors!(cloudGraph, session=sessionId, multisessions=caesarConfig.multiSession) + rmInstMultisessionPriors!(cloudGraph, session=sessionId, robot=robotId, user=userId, multisessions=caesarConfig.multiSession) println() println("====================SOLVE====================") solverStatus.currentStep = "Init_Solve" - fg = Caesar.initfg(sessionname=sessionId, cloudgraph=cloudGraph) + fg = Caesar.initfg(sessionname=sessionId, robotname=robotId, username=userId, cloudgraph=cloudGraph) - setBackendWorkingSet!(cloudGraph.neo4j.connection, sessionId) + setBackendWorkingSet!(cloudGraph.neo4j.connection, sessionId, robotId, userId) println("Get local copy of graph") solverStatus.currentStep = "Init_LocalGraphCopy" if fullLocalGraphCopy!(fg) + println("------ Count of nodes: $(fg.nodeIDs)") # (savejlds && itercount == 0) ? slamindbsavejld(fg, sessionId, itercount) : nothing itercount += 1 @@ -113,8 +116,9 @@ end Low-level call to iterate the SlamInDb solver for given number of iterations against a specific session and keyword parameters. """ function runDbSolver(cloudGraph::CloudGraphs.CloudGraph, - robotname::A, - sessionName::A; + robotId::A, + sessionId::A, + userId::A; N::Int=100, loopctrl::Vector{Bool}=Bool[true], iterations::Int=-1, @@ -127,7 +131,7 @@ function runDbSolver(cloudGraph::CloudGraphs.CloudGraph, while loopctrl[1] && (iterations > 0 || iterations == -1) # loopctrl for future use iterations = iterations == -1 ? iterations : iterations-1 # stop at 0 or continue indefinitely if -1 println("===================CONVERT===================") - fgl = Caesar.initfg(sessionname=sessionName, robotname=robotname, cloudgraph=cloudGraph) + fgl = Caesar.initfg(sessionname=sessionId, robotname=robotId, username=userId, cloudgraph=cloudGraph) updatenewverts!(fgl, N=N) println() @@ -136,13 +140,13 @@ function runDbSolver(cloudGraph::CloudGraphs.CloudGraph, println() println("================MULTI-SESSION================") - rmInstMultisessionPriors!(cloudGraph, session=sessionName, multisessions=multisession) + rmInstMultisessionPriors!(cloudGraph, session=sessionId, robot=robotId, user=userId, multisessions=multisession) println() println("====================SOLVE====================") - fgl = Caesar.initfg(sessionname=sessionName, cloudgraph=cloudGraph) + fgl = Caesar.initfg(sessionname=sessionId, robotname=robotId, username=userId, cloudgraph=cloudGraph) - setBackendWorkingSet!(cloudGraph.neo4j.connection, sessionName) + setBackendWorkingSet!(cloudGraph.neo4j.connection, sessionId, robotId, userId) println("get local copy of graph") @@ -192,6 +196,8 @@ function slamindb(;addrdict=nothing, N = parse(Int, addrdict["num particles"]) session = addrdict["session"] + robot = addrdict["robot"] + user = addrdict["user"] if !haskey(addrdict, "multisession") addrdict["multisession"]=String[] From 4dd5054f2fd4e9d897973ffcc3c74693690399b9 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Tue, 28 Aug 2018 23:57:55 -0500 Subject: [PATCH 12/47] Adding performance stats --- src/cloudgraphs/slamindb.jl | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/cloudgraphs/slamindb.jl b/src/cloudgraphs/slamindb.jl index 05badd26e..ef4bfe79f 100644 --- a/src/cloudgraphs/slamindb.jl +++ b/src/cloudgraphs/slamindb.jl @@ -76,7 +76,7 @@ function runSlamInDbOnSession( solverStatus.currentStep = "Init_LocalGraphCopy" if fullLocalGraphCopy!(fg) - println("------ Count of nodes: $(fg.nodeIDs)") + iterationStats.numNodes = length(fg.IDs) + length(fg.fIDs) #Variables and factors # (savejlds && itercount == 0) ? slamindbsavejld(fg, sessionId, itercount) : nothing itercount += 1 @@ -94,12 +94,16 @@ function runSlamInDbOnSession( else inferOverTreeR!(fg, tree, N=N) end - # savejlds ? slamindbsavejld(fg, sessionId, itercount) : nothing + else sleep(0.2) end + + # Notify iteration update. solverStatus.lastIterationDurationSeconds = toc() solverStatus.currentStep = "Idle" + iterationStats.endTimestamp = Dates.now() + iterationCompleteCallback(iterationStats) end solverStatus.isAttached = false From 7f57930c5f7bff20ab5f1c457ad590a59f0f9292 Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 29 Aug 2018 12:43:20 -0400 Subject: [PATCH 13/47] work in progress, still issues with fg on image data --- .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 71 +++++++++++-------- examples/wheeled/racecar/racecarUtils.jl | 12 ++-- 2 files changed, 48 insertions(+), 35 deletions(-) diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index 228cd6a2f..7a0b9d910 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -1,7 +1,7 @@ # Local compute version # add more julia processes -nprocs() < 5 ? addprocs(5-nprocs()) : nothing +# nprocs() < 5 ? addprocs(5-nprocs()) : nothing using Caesar, RoME, Distributions using YAML, JLD, HDF5 @@ -55,14 +55,26 @@ imgfolder = "images" # Figure export folder currdirtime = now() # currdirtime = "2018-08-14T00:52:01.534" -imgdir = joinpath(ENV["HOME"], "Pictures", "racecarimgs", "$(currdirtime)") +resultsdir = joinpath(ENV["HOME"], "Pictures", "racecarimgs") +imgdir = joinpath(resultsdir, "$(currdirtime)") mkdir(imgdir) mkdir(imgdir*"/tags") +camidxs = 0:5:1765 + +fid = open(imgdir*"/readme.txt", "w") +println(fid, datafolder) +println(fid, camidxs) +close(fid) + +fid = open(resultsdir*"/racecar.log", "a") +println(fid, "$(currdirtime), $datafolder") +close(fid) + # process images # camlookup = prepCamLookup(175:5:370) -camlookup = prepCamLookup(0:5:1765) +camlookup = prepCamLookup(camidxs) IMGS, TAGS = detectTagsViaCamLookup(camlookup, datafolder*imgfolder, imgdir) # IMGS[1] # TAGS[1] @@ -96,58 +108,59 @@ addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2, fcttype=DynPose2Pose2) tree = wipeBuildNewTree!(fg, drawpdf=true) inferOverTreeR!(fg,tree, N=N) -# plotKDE(fg, :l1, dims=[3]) +# plotKDE(fg, :l1, dims=[3]) # ls(fg) -# # val = getVal(fg, :l11) - # drawPosesLandms(fg, spscale=0.25) - # @async run(`evince bt.pdf`) + prev_psid = 0 # add other positions maxlen = (length(tag_bag)-1) # psid = 5 -for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen +for psid in 1:1:50 #maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen @show psym = Symbol("x$psid") - addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=VelPose2VelPose2, fcttype=DynPose2Pose2) + addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=VelPose2VelPose2, fcttype=DynPose2Pose2, autoinit=true) # writeGraphPdf(fg) - if psid % 30 == 0 || psid == maxlen + if psid % 1000 == 0 || psid == maxlen tree = wipeBuildNewTree!(fg, drawpdf=true) - inferOverTree!(fg,tree, N=N) + # inferOverTree!(fg,tree, N=N) end - # save factor graph for later testing and evaluation + ## save factor graph for later testing and evaluation IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym).jld") - ensureAllInitialized!(fg) - pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) #,xmin=-3,xmax=6,ymin=-5,ymax=2); - Gadfly.draw(PNG(joinpath(imgdir,"$(psym).png"),15cm, 10cm),pl) - pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); - Gadfly.draw(PNG(joinpath(imgdir,"hist_$(psym).png"),15cm, 10cm),pl) - pl = plotPose2Vels(fg, Symbol("$(psym)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0)) - Gadfly.draw(PNG(joinpath(imgdir,"vels_$(psym).png"),15cm, 10cm),pl) + # ensureAllInitialized!(fg) + # pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) #,xmin=-3,xmax=6,ymin=-5,ymax=2); + # Gadfly.draw(PNG(joinpath(imgdir,"$(psym).png"),15cm, 10cm),pl) + # pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); + # Gadfly.draw(PNG(joinpath(imgdir,"hist_$(psym).png"),15cm, 10cm),pl) + # pl = plotPose2Vels(fg, Symbol("$(psym)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0)) + # Gadfly.draw(PNG(joinpath(imgdir,"vels_$(psym).png"),15cm, 10cm),pl) # prepare for next iteration prev_psid = psid end -tree = wipeBuildNewTree!(fg, drawpdf=true) -# @async run(`evince bt.pdf`) -inferOverTree!(fg,tree, N=N) -inferOverTree!(fg,tree, N=N) - # save factor graph for later testing and evaluation IIF.savejld(fg, file=imgdir*"/racecar_fg_final.jld") -pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) #,xmin=-3,xmax=6,ymin=-5,ymax=2); -Gadfly.draw(PNG(joinpath(imgdir,"final.png"),15cm, 10cm),pl) -pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); -Gadfly.draw(PNG(joinpath(imgdir,"hist_final.png"),15cm, 10cm),pl) +tree = wipeBuildNewTree!(fg, drawpdf=true) +# # @async run(`evince bt.pdf`) +inferOverTreeR!(fg,tree, N=N) +# inferOverTree!(fg,tree, N=N) + + +# pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) #,xmin=-3,xmax=6,ymin=-5,ymax=2); +# Gadfly.draw(PNG(joinpath(imgdir,"final.png"),15cm, 10cm),pl) +# pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); +# Gadfly.draw(PNG(joinpath(imgdir,"hist_final.png"),15cm, 10cm),pl) + + + -0 #0 # # ls(fg, :l7) diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index 3c2ae66b9..c98ab51bc 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -24,7 +24,7 @@ end # add AprilTag sightings from this pose -function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1, lmtype=Point2, fcttype=Pose2Pose2, DAerrors=0.0 ) +function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1, lmtype=Point2, fcttype=Pose2Pose2, DAerrors=0.0, autoinit=true ) @show currtags = ls(fg)[2] for lmid in keys(posetags) @show lmsym = Symbol("l$lmid") @@ -52,7 +52,7 @@ function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1, lmtype=Point end if rand() > DAerrors # regular single hypothesis - addFactor!(fg, [pssym; lmsym], ppbr, autoinit=false) + addFactor!(fg, [pssym; lmsym], ppbr, autoinit=false, autoinit=autoinit) else # artificial errors to data association occur info("Forcing bad data association with $lmsym") @@ -60,23 +60,23 @@ function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1, lmtype=Point @show ll2 = setdiff(ll, [lmsym]) @show daidx = round(Int, (length(ll2)-1)*rand()+1) @show rda = ll2[daidx] - addFactor!(fg, [pssym; lmsym; rda], ppbr, autoinit=false, multihypo=[1.0;0.5;0.5]) + addFactor!(fg, [pssym; lmsym; rda], ppbr, autoinit=false, multihypo=[1.0;0.5;0.5], autoinit=autoinit) end end nothing end -function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odotype=Pose2Pose2, fcttype=Pose2Pose2, DAerrors=0.0) +function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odotype=Pose2Pose2, fcttype=Pose2Pose2, DAerrors=0.0, autoinit=true) prev_pssym = Symbol("x$(prev_psid)") new_pssym = Symbol("x$(new_psid)") # first pose with zero prior if odotype == Pose2Pose2 addNode!(fg, new_pssym, Pose2) - addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.4;0.1;0.4].^2)))) + addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.4;0.1;0.4].^2))), autoinit=autoinit) elseif odotype == VelPose2VelPose2 addNode!(fg, new_pssym, DynPose2(ut=round(Int, 200_000*(new_psid)))) addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.4;0.07;0.1].^2)), - MvNormal(zeros(2),diagm([0.2;0.2].^2)))) + MvNormal(zeros(2),diagm([0.2;0.2].^2))), autoinit=autoinit) end addApriltags!(fg, new_pssym, pose_tag_bag, lmtype=lmtype, fcttype=fcttype, DAerrors=DAerrors) From 7be4f16219379da02d4c49ab708102e4c89f1771 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Wed, 29 Aug 2018 21:54:45 -0500 Subject: [PATCH 14/47] Updated loop message --- src/cloudgraphs/slamindb.jl | 126 +++++++++++++++++++----------------- 1 file changed, 67 insertions(+), 59 deletions(-) diff --git a/src/cloudgraphs/slamindb.jl b/src/cloudgraphs/slamindb.jl index ef4bfe79f..f26649019 100644 --- a/src/cloudgraphs/slamindb.jl +++ b/src/cloudgraphs/slamindb.jl @@ -44,66 +44,74 @@ function runSlamInDbOnSession( itercount = 0 while ((iterations > 0 || iterations == -1) && solverStatus.isAttached) - iterations = iterations == -1 ? iterations : iterations-1 # stop at 0 or continue indefinitely if -1 - - iterationStats = IterationStatistics() - tic() - solverStatus.iteration = itercount - - println("===================CONVERT===================") - solverStatus.currentStep = "Prep_Convert" - fg = Caesar.initfg(sessionname=sessionId, robotname=robotId, username=userId, cloudgraph=cloudGraph) - updatenewverts!(fg, N=N) - println() - - println("=============ENSURE INITIALIZED==============") - solverStatus.currentStep = "Prep_EnsureInitialized" - ensureAllInitialized!(fg) - println() - - println("================MULTI-SESSION================") - solverStatus.currentStep = "Prep_MultiSession" - rmInstMultisessionPriors!(cloudGraph, session=sessionId, robot=robotId, user=userId, multisessions=caesarConfig.multiSession) - println() - - println("====================SOLVE====================") - solverStatus.currentStep = "Init_Solve" - fg = Caesar.initfg(sessionname=sessionId, robotname=robotId, username=userId, cloudgraph=cloudGraph) - - setBackendWorkingSet!(cloudGraph.neo4j.connection, sessionId, robotId, userId) - - println("Get local copy of graph") - - solverStatus.currentStep = "Init_LocalGraphCopy" - if fullLocalGraphCopy!(fg) - iterationStats.numNodes = length(fg.IDs) + length(fg.fIDs) #Variables and factors - # (savejlds && itercount == 0) ? slamindbsavejld(fg, sessionId, itercount) : nothing - itercount += 1 - - println("-------------Ensure Initialization-----------") - solverStatus.currentStep = "Solve_EnsureInitialized" - ensureAllInitialized!(fg) - - println("------------Bayes (Junction) Tree------------") - solverStatus.currentStep = "Solve_BuildBayesTree" - tree = wipeBuildNewTree!(fg,drawpdf=drawbayestree) - - solverStatus.currentStep = "Solve_InferOverTree" - if !isRecursiveSolver - inferOverTree!(fg, tree, N=N) - else - inferOverTreeR!(fg, tree, N=N) - end - - else - sleep(0.2) + iterationStats = IterationStatistics() + try + iterations = iterations == -1 ? iterations : iterations-1 # stop at 0 or continue indefinitely if -1 + + tic() + solverStatus.iteration = itercount + + println("===================CONVERT===================") + solverStatus.currentStep = "Prep_Convert" + fg = Caesar.initfg(sessionname=sessionId, robotname=robotId, username=userId, cloudgraph=cloudGraph) + updatenewverts!(fg, N=N) + println() + + println("=============ENSURE INITIALIZED==============") + solverStatus.currentStep = "Prep_EnsureInitialized" + ensureAllInitialized!(fg) + println() + + println("================MULTI-SESSION================") + solverStatus.currentStep = "Prep_MultiSession" + rmInstMultisessionPriors!(cloudGraph, session=sessionId, robot=robotId, user=userId, multisessions=caesarConfig.multiSession) + println() + + println("====================SOLVE====================") + solverStatus.currentStep = "Init_Solve" + fg = Caesar.initfg(sessionname=sessionId, robotname=robotId, username=userId, cloudgraph=cloudGraph) + + setBackendWorkingSet!(cloudGraph.neo4j.connection, sessionId, robotId, userId) + + println("Get local copy of graph") + + solverStatus.currentStep = "Init_LocalGraphCopy" + if fullLocalGraphCopy!(fg) + iterationStats.numNodes = length(fg.IDs) + length(fg.fIDs) #Variables and factors + # (savejlds && itercount == 0) ? slamindbsavejld(fg, sessionId, itercount) : nothing + itercount += 1 + + println("-------------Ensure Initialization-----------") + solverStatus.currentStep = "Solve_EnsureInitialized" + ensureAllInitialized!(fg) + + println("------------Bayes (Junction) Tree------------") + solverStatus.currentStep = "Solve_BuildBayesTree" + tree = wipeBuildNewTree!(fg,drawpdf=drawbayestree) + + solverStatus.currentStep = "Solve_InferOverTree" + if !isRecursiveSolver + inferOverTree!(fg, tree, N=N) + else + inferOverTreeR!(fg, tree, N=N) + end + + else + sleep(0.2) + end + + # Notify iteration update. + solverStatus.lastIterationDurationSeconds = toc() + solverStatus.currentStep = "Idle" + iterationStats.result = "GOOD" + catch ex + stack = catch_stacktrace() + msg = "ERROR\r\nMessage: $(ex.msg)\r\nStacktrace:\r\n$(stack)" + iterationStats.result = msg + finally + iterationStats.endTimestamp = Dates.now() + iterationCompleteCallback(iterationStats) end - - # Notify iteration update. - solverStatus.lastIterationDurationSeconds = toc() - solverStatus.currentStep = "Idle" - iterationStats.endTimestamp = Dates.now() - iterationCompleteCallback(iterationStats) end solverStatus.isAttached = false From fde9779a18330450236ba16be28239d4da647ac1 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Thu, 6 Sep 2018 23:04:37 -0500 Subject: [PATCH 15/47] Updates, trying to find all occurrences --- docs/src/index.md | 2 +- examples/database/foveation.jl | 4 +-- examples/database/templatedbinteraction.jl | 5 ++- .../PlottingToolsForFactorGraphs.jl | 6 ++-- .../rov/highend/lcmserver/drawtoolsfromdb.jl | 6 ++-- src/cloudgraphs/CloudGraphIntegration.jl | 20 ++++++++---- src/cloudgraphs/FoveationUtils.jl | 32 ++++++++++++------- src/cloudgraphs/MultisessionUtils.jl | 4 +-- 8 files changed, 49 insertions(+), 30 deletions(-) diff --git a/docs/src/index.md b/docs/src/index.md index 31fe13275..7ff0397cb 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -58,7 +58,7 @@ drawdbdirector() # from database held factor graph * [Foveation queries](http://people.csail.mit.edu/spillai/projects/cloud-graphs/2017-icra-cloudgraphs.pdf) to quickly organize, extract and work with big data blobs, for example looking at images from multiple sessions predicted to see the same point `[-9.0,9.0]` in the map: ```julia -neoids, syms = foveateQueryToPoint(cloudGraph,["SESS21";"SESS38";"SESS45"], point=[-9.0;9.0], fovrad=0.5 ) +neoids, syms = foveateQueryToPoint(cloudGraph,["SESS21";"SESS38";"SESS45"], "robot", "user" point=[-9.0;9.0], fovrad=0.5 ) for neoid in neoids cloudimshow(cloudGraph, neoid=neoid) end diff --git a/examples/database/foveation.jl b/examples/database/foveation.jl index d6b615ae3..a7bc48719 100644 --- a/examples/database/foveation.jl +++ b/examples/database/foveation.jl @@ -12,9 +12,9 @@ cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict) - +#TODO: Please replace with correct robot and user name. neoids, syms = foveateQueryToPoint(cloudGraph, - ["SESSTURT21";"SESSTURT38";"SESSTURT45"], + ["SESSTURT21";"SESSTURT38";"SESSTURT45"], "robot", "user" point=[-9.0;9.0], fovrad=0.5) diff --git a/examples/database/templatedbinteraction.jl b/examples/database/templatedbinteraction.jl index e47d3c7d4..6862f3640 100644 --- a/examples/database/templatedbinteraction.jl +++ b/examples/database/templatedbinteraction.jl @@ -5,6 +5,8 @@ using IncrementalInference include(joinpath(dirname(@__FILE__),"blandauthremote.jl")) addrdict["session"] = "SESS??" +addrdict["robot"] = "robot" +addrdict["user"] = "user" cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict) @@ -16,7 +18,8 @@ savejld(fg, file="somefg.jld") plotKDE(fg, :x1, dims=[1;2], api=localapi) fncvar = getfnctype(getVert(fg,fg.fIDs[:x1710l200050])) -ret = whosNear2D(cloudGraph, "SESS??", x=21.7, y=-54.0) +# TODO: Please fix with correct session for example. +ret = whosNear2D(cloudGraph, "SESS??", robot, user, x=21.7, y=-54.0) plotLocalProduct(fg, :l200050, dims=[1;2], api=localapi) diff --git a/examples/database/visualizations/PlottingToolsForFactorGraphs.jl b/examples/database/visualizations/PlottingToolsForFactorGraphs.jl index 79474e503..387e4188e 100644 --- a/examples/database/visualizations/PlottingToolsForFactorGraphs.jl +++ b/examples/database/visualizations/PlottingToolsForFactorGraphs.jl @@ -5,6 +5,8 @@ using IncrementalInference include(joinpath(dirname(@__FILE__),"blandauthremote.jl")) addrdict["session"] = "SESSSHARK_16_11_14" # "SESSROX" +addrdict["robot"] = "robot" +addrdict["user"] = "user" cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict) @@ -49,8 +51,8 @@ h2 = plotKDE(fncvar.bearing, xlbl="Bearing [rad]", fill=true ) Gadfly.draw(PDF("factor_x1710l200050_stack.pdf",15cm,20cm),vstack(h1,h2)) - -ret = whosNear2D(cloudGraph, "SESSSHARK_16_11_14", x=21.7, y=-54.0) +# TODO: Please fix with correct session for example. +ret = whosNear2D(cloudGraph, "SESSSHARK_16_11_14", "robot", "user" x=21.7, y=-54.0) sort(collect(keys(ret))) diff --git a/examples/marine/rov/highend/lcmserver/drawtoolsfromdb.jl b/examples/marine/rov/highend/lcmserver/drawtoolsfromdb.jl index d7473638b..e64f4f7b3 100644 --- a/examples/marine/rov/highend/lcmserver/drawtoolsfromdb.jl +++ b/examples/marine/rov/highend/lcmserver/drawtoolsfromdb.jl @@ -5,6 +5,8 @@ using Caesar include(joinpath(dirname(@__FILE__),"..","database","blandauthremote.jl")) addrdict["session"] = "SESS??" +addrdict["robot"] = "robot" +addrdict["user"] = "user" cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict) @@ -35,8 +37,8 @@ Gadfly.draw(PDF("test.pdf",40cm, 40cm),pl) using IncrementalInference fncvar = getfnctype(getVert(fg,fg.fIDs[:x1710l200050])) - -ret = whosNear2D(cloudGraph, "SESS??", x=9.8, y=15.2, dist=0.5) +#TODO: Please fix with correct session for example +ret = whosNear2D(cloudGraph, "SESS??", "robot", "user", x=9.8, y=15.2, dist=0.5) plotLocalProduct(fg, :x1, dims=[1;2], api=localapi) plotKDEresiduals(fg, :x1x2, dims=[1;2], api=localapi) diff --git a/src/cloudgraphs/CloudGraphIntegration.jl b/src/cloudgraphs/CloudGraphIntegration.jl index 871590dc3..f3100ebc9 100644 --- a/src/cloudgraphs/CloudGraphIntegration.jl +++ b/src/cloudgraphs/CloudGraphIntegration.jl @@ -44,7 +44,7 @@ export """ $(SIGNATURES) -Run Neo4j Cypher queries on the cloudGraph database, andreturn Tuple with the +Run Neo4j Cypher queries on the cloudGraph database, and return Tuple with the unparsed (results, loadresponse). """ function executeQuery( @@ -543,6 +543,7 @@ function buildSubGraphIdsQuery(; lbls::Vector{AS}=String[""], session::AS="", robot::AS="", + user::AS="", label::AS="", reqready::Bool=true, ready::Int=1, @@ -552,14 +553,15 @@ function buildSubGraphIdsQuery(; # sn = length(session) > 0 ? ":"*session : "" rn = length(robot) > 0 ? ":"*robot : "" + un = length(user) > 0 ? ":"*user : "" lb = length(label) > 0 ? ":"*label : "" query = "" for nei in 0:(neighbors) - query *= "match (n0$(sn)$(rn)$(lb))" + query *= "match (n0$(sn)$(rn)$(un)$(lb))" outerd = 0 for d in 1:(nei) - query *= "-[:DEPENDENCE]-(n$(d)$(sn)$(rn)$(lb))" + query *= "-[:DEPENDENCE]-(n$(d)$(sn)$(rn)$(un)$(lb))" outerd = d end query *= " " @@ -596,7 +598,7 @@ function getLblExVertexNeoIDs( neighbors::Int=0 ) where {AS <: AbstractString} # - query = buildSubGraphIdsQuery(lbls=lbls, session=session, robot=robot, label=label, neighbors=neighbors, reqready=reqready, ready=ready, reqbackendset=reqbackendset, backendset=backendset) + query = buildSubGraphIdsQuery(lbls=lbls, session=session, robot=robot, user=user, label=label, neighbors=neighbors, reqready=reqready, ready=ready, reqbackendset=reqbackendset, backendset=backendset) cph, = executeQuery(conn, query) ret = Array{Tuple{Int64,Int64,Symbol},1}() @@ -618,11 +620,15 @@ function getExVertexNeoIDs( ready::Int=1, backendset::Int=1, session::AS="", + robot::AS="", + user::AS="", reqbackendset::Bool=true ) where {AS <: AbstractString} # sn = length(session) > 0 ? ":"*session : "" + rn = length(robot) > 0 ? ":"*robot : "" + un = length(user) > 0 ? ":"*user : "" lb = length(label) > 0 ? ":"*label : "" - query = "match (n$(sn)$(lb)) where n.ready=$(ready) and exists(n.exVertexId)" + query = "match (n$(sn)$(rn)$(un)$(lb)) where n.ready=$(ready) and exists(n.exVertexId)" query = reqbackendset ? query*" and n.backendset=$(backendset)" : query query = query*" return n.exVertexId, id(n), n.label" @@ -1284,8 +1290,8 @@ function db2jld(filename::AbstractString; addrdict::VoidUnion{Dict{AbstractStrin end -function deleteServerSession!(cloudGraph::CloudGraph, session::AbstractString) - query = "match (n:$(session)) +function deleteServerSession!(cloudGraph::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString) + query = "match (n:$(session):$robot:$user) detach delete n return count(n)" return executeQuery(cloudGraph.neo4j.connection, query) diff --git a/src/cloudgraphs/FoveationUtils.jl b/src/cloudgraphs/FoveationUtils.jl index 43b780325..952dfd369 100644 --- a/src/cloudgraphs/FoveationUtils.jl +++ b/src/cloudgraphs/FoveationUtils.jl @@ -8,11 +8,11 @@ export """ - whosNear2D(cg::CloudGraph, session::AbstractString; x,y,yaw,dist,angle ) + $(SIGNATURES) Find vertices near the point specified and return dictionary of symbol to Neo4j ID pairs. """ - function whosNear2D(cg::CloudGraph, session::AbstractString; + function whosNear2D(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString; x=nothing, y=nothing, yaw=nothing, @@ -23,7 +23,7 @@ export error("please give some info on where you want to run the spacial query, xyYaw.") end # Build the query - query = "match (n:$(session)) where exists(n.MAP_est) " + query = "match (n:$(session):$robot:$user) where exists(n.MAP_est) " query = x==nothing ? query : query*"and abs(n.MAP_est[0] - $(x)) < $(dist) " query = y==nothing ? query : query*"and abs(n.MAP_est[1] - $(y)) < $(dist) " query = yaw==nothing ? query : query*"and abs(n.MAP_est[2] - $(yaw)) < $(angle) " @@ -41,11 +41,11 @@ export end """ - whosNear3D(cg::CloudGraph, session::AbstractString; x,y,z,roll, pitch,yaw,dist,angle ) + $(SIGNATURES) Find vertices near the point specified and return dictionary of symbol to Neo4j ID pairs. """ - function whosNear3D(cg::CloudGraph, session::AbstractString; + function whosNear3D(cg::CloudGraph, session::AbstractString, robot::AbstractString, user::AbstractString; x=nothing, y=nothing, z=nothing, @@ -59,7 +59,7 @@ export error("please give some info on where you want to run the spacial query, xyzRPY.") end # Build the query - query = "match (n:$(session)) where exists(n.MAP_est) " + query = "match (n:$(session):$robot:$user) where exists(n.MAP_est) " query = x==nothing ? query : query*"and abs(n.MAP_est[0] - $(x)) < $(dist) " query = y==nothing ? query : query*"and abs(n.MAP_est[1] - $(y)) < $(dist) " query = z==nothing ? query : query*"and abs(n.MAP_est[2] - $(z)) < $(dist) " @@ -79,19 +79,23 @@ export end - +""" + $(SIGNATURES) +""" function foveateQueryToPoint{T <: AbstractString}(cg::CloudGraph, - sessions::Vector{T}; + sessions::Vector{T}, + robot::T, + user::T; point::Vector{Float64}=[0.0;0.0], minrange::Number=1.5, maxrange::Number=5, fovrad::Number=0.3 ) # neoids, syms = Vector{Int}(), Vector{Symbol}() loadtx = transaction(cg.neo4j.connection) nms = length(sessions) - query = "MATCH (n) WHERE (" + query = "MATCH (n:$robot:$user) WHERE (" cnt = 0 for session in sessions - query *= "(n:$(session)) " + query *= "(n:$(session):$robot:$user) " cnt+=1 if cnt < nms query *= " or " @@ -117,7 +121,7 @@ function foveateQueryToPoint{T <: AbstractString}(cg::CloudGraph, abs(atan2( ($(point[2])-n.MAP_est[1]), ($(point[1])-n.MAP_est[0])) - n.MAP_est[2] - 6.28) < $(fovrad)) RETURN id(n), n.label" - cph = loadtx(query, submit=true) + cph, = executeQuery(cg.neo4j.connection, query) for res in cph.results[1]["data"] push!(neoids, res["row"][1] ) push!(syms, Symbol(res["row"][2]) ) @@ -126,12 +130,16 @@ function foveateQueryToPoint{T <: AbstractString}(cg::CloudGraph, end function foveateQueryToPoint(cg::CloudGraph, - session::AbstractString; + session::AbstractString, + robot::AbstractString, + user::AbstractString; point::Vector{Float64}=[0.0;0.0], minrange::Number=1.5, maxrange::Number=5, fovrad::Number=0.3 ) # foveateQueryToPoint(cg::CloudGraph, [session], + robot, + user, point=point, minrange=minrange, maxrange=maxrange, fovrad=fovrad ) end diff --git a/src/cloudgraphs/MultisessionUtils.jl b/src/cloudgraphs/MultisessionUtils.jl index ad6f7c5b7..99ed298f0 100644 --- a/src/cloudgraphs/MultisessionUtils.jl +++ b/src/cloudgraphs/MultisessionUtils.jl @@ -154,11 +154,9 @@ end Return Dict{Symbol, Int} of vertex symbol to Neo4j node ID of MULTISESSION constraints in this `fgl.sessionname`. """ function findExistingMSConstraints(fgl::FactorGraph) - loadtx = transaction(fgl.cg.neo4j.connection) query = "match (n:$(fgl.sessionname):$(fgl.robotname):$(fgl.username):MULTISESSION) return id(n), n.exVertexId, n.label" - cph = loadtx(query, submit=true) - + cph, = executeQuery(cg, query) # parse the response into a dictionary existingms = Dict{Symbol, Int}() # symlbl => neoid for res in cph.results[1]["data"] From 06405b985759e35c93019ce3a984f2c5c536d4a0 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Thu, 6 Sep 2018 23:41:32 -0500 Subject: [PATCH 16/47] WIP on example --- test/fourdoortestcloudgraph.jl | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/test/fourdoortestcloudgraph.jl b/test/fourdoortestcloudgraph.jl index 6827bce99..8dda6ae5e 100644 --- a/test/fourdoortestcloudgraph.jl +++ b/test/fourdoortestcloudgraph.jl @@ -11,14 +11,16 @@ using KernelDensityEstimate # dbpwd = "" # mongoaddress = "localhost" session = "SESSTEST" +robot = "TESTROBOT" +user = "TESTUSER" include(joinpath(dirname(@__FILE__) ,"blandauth.jl")) Nparticles = 200 println("Attempting to solve session $(session)...") configuration = CloudGraphs.CloudGraphConfiguration(dbaddress, 7474, dbusr, dbpwd, mongoaddress, 27017, false, "", ""); -cloudGraph = connect(configuration, encodePackedType, getpackedtype, decodePackedType); +cloudGraph = connect(configuration, IncrementalInference.encodePackedType, Caesar.getpackedtype, IncrementalInference.decodePackedType); # register types of interest in CloudGraphs -registerGeneralVariableTypes!(cloudGraph) +# registerGeneralVariableTypes!(cloudGraph) Caesar.usecloudgraphsdatalayer!() # Uncomment out for command line operation @@ -29,16 +31,17 @@ Caesar.usecloudgraphsdatalayer!() # this is being replaced by cloudGraph, added here for development period -fg = Caesar.initfg(sessionname=session, cloudgraph=cloudGraph) +fg = Caesar.initfg(sessionname=session, robotname=robot, username=user, cloudgraph=cloudGraph) # Robot navigation and inference type stuff N=Nparticles -doors = [-100.0;0.0;100.0;300.0]' -cov = [3.0] +doors = reshape([-100.0;0.0;100.0;300.0],1,4) +cov = 3.0*ones(1,1) +#TODO: WIP # robot style, add first pose vertex -v1 = addNode!(fg,:x1,doors,N=N,labels=["POSE"]) +v1 = addNode!(fg,:x1,ContinuousScalar,N=N, labels=["POSE"]) # add a prior for the initial position of the robot f0 = addFactor!(fg,[v1], Obsv2(doors, cov', [1.0])) From 2c9a6be64f93da33630801edf8e87ac8cd72d9f2 Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 7 Sep 2018 14:13:40 -0400 Subject: [PATCH 17/47] starting wheel odo --- examples/wheeled/parametricWheelOdo.jl | 56 ++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 examples/wheeled/parametricWheelOdo.jl diff --git a/examples/wheeled/parametricWheelOdo.jl b/examples/wheeled/parametricWheelOdo.jl new file mode 100644 index 000000000..5834dbecd --- /dev/null +++ b/examples/wheeled/parametricWheelOdo.jl @@ -0,0 +1,56 @@ +using TransformUtils + + + +function uteOdom(x::Array{Float64,1}, vc::Float64, alpha::Float64, dt::Float64) + L = 2.83; H=076; b=0.5;a=3.78; + d = [(vc*cos(x[3])-vc/L*tan(x[3])*(a*sin(x[3])+b*cos(x[3]))); + (vc*sin(x[3])+vc/L*tan(x[3])*(a*cos(x[3])-b*sin(x[3]))); + vc/L*tan(alpha)] + x += dt*d + x[3] = wrapRad(x[3]) + return x +end + +function allOdo(sensors::Array{Float64,2}) + odo = zeros(size(sensors,1),3) + T0 = 0.0 + for i in 2:size(sensors,1) + odo[i,:]=uteOdom(vec(odo[i-1,:]),sensors[i,2],sensors[i,3],sensors[i,1]-T0) + T0 = sensors[i,1] + end + + return odo +end + +vc(v::Float64, alpha::Float64; L=2.80381, H=0.828329 ) = v./(1.0-tan(alpha)*H/L) +ve(v::Float64, alpha::Float64; L=2.80381, H=0.828329 ) = v.*(1.0-tan(alpha).*H/L) +dPhi(v::Float64, alpha::Float64; L=2.80381) = 1.0*v*tan(alpha)/L + +function compensateRawsensors(drs::Array{Float64,1}; + L=2.80381, H=0.828329, whlsf=0.94, strsf=1.0199, strbi=0.00159) + return whlsf*drs[2], strsf*drs[3]+strbi +end + +function uteOdomEasy(x::Array{Float64,1}, whlspd::Float64, strangl::Float64, dt::Float64; L=2.80381, H=0.828329) + wTb = SE2(x) + v = vc(whlspd,strangl,L=L,H=H) + dph = dPhi(v,strangl,L=L) + #dph = v*tan(strangl) + bTnb = SE2(dt*[v;0.0;dph]) + wTnb = wTb*bTnb + return vec(se2vee(wTnb)) +end + +# perform integration over all data, sensors is rows of data, by columns of sensors +function allOdoEasy(sensors::Array{Float64,2}; + L=2.80381, H=0.828329, whlsf=0.94, strsf=1.0199, strbi=0.00159) + #msensors = [sensors[:,1]';x[1]*sensors[:,2]';x[5]*sensors[:,3]'+x[2]]'; + odo = zeros(size(sensors,1),3) + for i in 2:size(sensors,1) + whlspd, strang = compensateRawsensors(vec(sensors[i,:]), L=L,H=H, whlsf=whlsf, strbi=strbi,strsf=strsf) + odo[i,:] = uteOdomEasy(vec(odo[i-1,:]), whlspd, strang, (sensors[i,1]-sensors[i-1,1]), L=L, H=H ) + end + + return odo +end From 59893ceb4f9280968ba736b7d62c5961311dbb5b Mon Sep 17 00:00:00 2001 From: GearsAD Date: Fri, 7 Sep 2018 22:55:36 -0500 Subject: [PATCH 18/47] WIP on fourdoorexample updates --- test/fourdoortestcloudgraph.jl | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/test/fourdoortestcloudgraph.jl b/test/fourdoortestcloudgraph.jl index 8dda6ae5e..2b992e626 100644 --- a/test/fourdoortestcloudgraph.jl +++ b/test/fourdoortestcloudgraph.jl @@ -44,11 +44,14 @@ cov = 3.0*ones(1,1) v1 = addNode!(fg,:x1,ContinuousScalar,N=N, labels=["POSE"]) # add a prior for the initial position of the robot -f0 = addFactor!(fg,[v1], Obsv2(doors, cov', [1.0])) +fct = Obsv2(doors, cov, [1.0]) +# TODO: Fix here - what's up with it failing to encode? +#WIP +f1 = addFactor!(fg,[v1], fct) # add second pose vertex tem = 2.0*randn(1,N)+getVal(v1)+50.0 -v2 = addNode!(fg, :x2, tem, N=N,labels=["POSE"]) +v2 = addNode!(fg, :x2, ContinuousScalar, N=N,labels=["POSE"]) # now add the odometry factor between them f1 = addFactor!(fg,[v1;v2],Odo([50.0]',[2.0]',[1.0])) From 2af95bf560ab702dc1770dbdc8634bb86b4c5993 Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 8 Sep 2018 15:52:07 -0400 Subject: [PATCH 19/47] wip --- examples/wheeled/victoriapark_stepbystep.jl | 6 +----- src/Caesar.jl | 1 + src/SlamConvenienceFunctions.jl | 1 - 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/examples/wheeled/victoriapark_stepbystep.jl b/examples/wheeled/victoriapark_stepbystep.jl index 9a3956342..87e3eb1be 100644 --- a/examples/wheeled/victoriapark_stepbystep.jl +++ b/examples/wheeled/victoriapark_stepbystep.jl @@ -23,10 +23,6 @@ function evalLikelihood(fg::FactorGraph, sym::Symbol, points::Array{Float64,2}) evaluateDualTree(p, (points)) end -function batchSolve(fgl) - tree = prepBatchTree!(fgl, drawpdf=true); - @time inferOverTree!(fgl,tree, N=100); -end @@ -142,7 +138,7 @@ writeGraphPdf(fg) @async run(`evince fg.pdf`) -batchSolve(fg) +batchSolve!(fg) pl1=drawPosesLandms(fg) draw(PDF("after.pdf",20cm,20cm),pl1) diff --git a/src/Caesar.jl b/src/Caesar.jl index 10b169e0e..56a1b66cc 100644 --- a/src/Caesar.jl +++ b/src/Caesar.jl @@ -3,6 +3,7 @@ module Caesar # import RoME: initfg # collision on RoME.initfg() since no parameters are given in both RoME and Caesar import Distributions: Normal import RoME: getRangeKDEMax2D, getLastPose, initfg +import IncrementalInference: batchSolve! using RoME, diff --git a/src/SlamConvenienceFunctions.jl b/src/SlamConvenienceFunctions.jl index f674f9ce2..39f92eada 100644 --- a/src/SlamConvenienceFunctions.jl +++ b/src/SlamConvenienceFunctions.jl @@ -118,7 +118,6 @@ function parseLandmarkXY!(slam::SLAMWrapper, sp2::Array{SubString{AbstractString nothing end - function batchSolve!(slam::SLAMWrapper, sp2::Array{SubString{AbstractString},1}) println("batchSolve -- wiping tree and solving") slam.tree = wipeBuildNewTree!(slam.fg) From aed1d99adc98f59bf97068d9a36e58ee3055b010 Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 8 Sep 2018 15:56:28 -0400 Subject: [PATCH 20/47] initial commit --- examples/marine/auv/Sandshark/Sandshark_2.jl | 1 + 1 file changed, 1 insertion(+) create mode 100644 examples/marine/auv/Sandshark/Sandshark_2.jl diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl new file mode 100644 index 000000000..7387174ca --- /dev/null +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -0,0 +1 @@ +# new Sandshark example From b426f422b4c72531db1cf34ed714100e690dcccf Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sat, 8 Sep 2018 17:47:20 -0500 Subject: [PATCH 21/47] Working session for sandshark --- examples/marine/auv/Sandshark/Sandshark_2.jl | 162 +++++++++++++++++++ 1 file changed, 162 insertions(+) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index 7387174ca..81acaee19 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -1 +1,163 @@ # new Sandshark example + +using Caesar, RoME, KernelDensityEstimate +using Interpolations + +using RoMEPlotting +using Gadfly, DataFrames + +datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") +matcheddir = joinpath(datadir, "matchedfilter", "particles") +beamdir = joinpath(datadir, "beamformer", "particles") +# matchedFilter +# 1531152812000000000.csv +matchedFiles = readdir(matcheddir) +beamFiles = readdir(beamdir) +timestamps = map(a -> parse(a[1:end-4]), matchedFiles) +beamtimestamps = map(a -> parse(a[1:end-4]), beamFiles) +unionTs = intersect(timestamps, beamtimestamps) + +# load all data into dictionaries +rangedata = Dict{Int,Array{Float64}}() +azidata = Dict{Int,Array{Float64}}() +# rangeT0 = 1531152812000000000 + +mints = min(timestamps...) +maxts = max(timestamps...) + +function loaddircsvs(datadir) + # https://docs.julialang.org/en/v0.6.1/stdlib/file/#Base.Filesystem.walkdir + datadict = Dict{Int, Array{Float64}}() + for (root, dirs, files) in walkdir(datadir) + # println("Files in $root") + for file in files + # println(joinpath(root, file)) # path to files + data = readdlm(joinpath(root, file),',') + datadict[parse(Int,split(file,'.')[1])/1000000] = data + end + end + return datadict +end + +rangedata = loaddircsvs(matcheddir) +rangekeys = sort(collect(keys(rangedata))) + +azidata = loaddircsvs(beamdir) +azikeys = sort(collect(keys(azidata))) + + +# NAV data +navdata = Dict{Int, Vector{Float64}}() +navfile = readdlm(joinpath(datadir, "nav_data.csv")) +for row in navfile + s = split(row, ",") + id = round(Int, 1000*parse(s[1])) + # round(Int, 1000 * parse(s[1])) = 1531153292381 + navdata[id] = parse.(s) +end +# navdata[first(keys(navdata))] + +navkeys = sort(collect(keys(navdata))) + +# latency is crazy - yep keys are timestamps +# NAV colums are X,Y = 7,8 +# lat,long = 9,10 +# e.g. +# 10-element Array{Float64,1}: +# 1.53115e9 +# -6.33978 +# 0.449772 +# 129.62 +# 0.650937 +# 1.156 +# 51.9071 +# -33.1335 +# 42.3582 +# -71.087 + + + + + + + +X = Float64[] +Y = Float64[] +for id in navkeys + push!(X, getindex(navdata[id],7)) + push!(Y, getindex(navdata[id],8)) +end +# navdf = DataFrame( +# ts = navkeys - navkeys[1], +# x = X, +# y = Y +# ) +# pl = Gadfly.plot(navdf, x=:x, y=:y, Geom.path()) +# Gadfly.draw(Gadfly.PNG("/tmp/navss.png", 30cm, 20cm),pl) + + +interp_x = LinearInterpolation(navkeys, X) +interp_y = LinearInterpolation(navkeys, Y) + +interp_x(sum(navkeys[1:2])/2) + +navkeys[1] +epochs[1] + +interp_x(epochs[1]) + +## SELECT SEGMENT OF DATA TO WORK WITH +ppbrDict = Dict{Int, Pose2Point2BearingRange}() + + +epochs = rangekeys # [51:60] +GPS = Dict{Int, Vector{Float64}}() +# almost there -- be right back. +for ep in epochs + x, y = interp_x(ep), interp_y(ep) + GPS[ep] = [x;y] + rangepts = rangedata[ep][:] + rangeprob = kde!(rangepts) + azipts = azidata[ep][:,1] + aziprob = kde!(azipts) + # prep the factor functions + ppbrDict[ep] = Pose2Point2BearingRange(aziprob, rangeprob) +end + + +GPS_X[7:8] +GPS_Y[7:8] + + +## build the factor graph + +fg = initfg() + +# Add a central beacon +addNode!(fg, :l1, Point2) +# addFactor() +index = 0 +for ep in epochs + # Correct variable type Pose2? + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + addFactor!(fg, [curvar; :l1], ppbrDict[ep]) # that makes sense - gotcha -- I was on odo sorry + index+=1 +end + + +writeGraphPdf(fg) + + +batchSolve!(fg) + + + + + + + + + + +# From eccc6bcbb95f9c42d6b089df7f49c12236184f5d Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 9 Sep 2018 10:01:26 -0500 Subject: [PATCH 22/47] Cleanup --- examples/marine/auv/Sandshark/Sandshark_2.jl | 67 ++------------------ 1 file changed, 7 insertions(+), 60 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index 81acaee19..ca0d2006c 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -5,25 +5,11 @@ using Interpolations using RoMEPlotting using Gadfly, DataFrames +using ProgressMeter datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") matcheddir = joinpath(datadir, "matchedfilter", "particles") beamdir = joinpath(datadir, "beamformer", "particles") -# matchedFilter -# 1531152812000000000.csv -matchedFiles = readdir(matcheddir) -beamFiles = readdir(beamdir) -timestamps = map(a -> parse(a[1:end-4]), matchedFiles) -beamtimestamps = map(a -> parse(a[1:end-4]), beamFiles) -unionTs = intersect(timestamps, beamtimestamps) - -# load all data into dictionaries -rangedata = Dict{Int,Array{Float64}}() -azidata = Dict{Int,Array{Float64}}() -# rangeT0 = 1531152812000000000 - -mints = min(timestamps...) -maxts = max(timestamps...) function loaddircsvs(datadir) # https://docs.julialang.org/en/v0.6.1/stdlib/file/#Base.Filesystem.walkdir @@ -40,11 +26,8 @@ function loaddircsvs(datadir) end rangedata = loaddircsvs(matcheddir) -rangekeys = sort(collect(keys(rangedata))) - azidata = loaddircsvs(beamdir) -azikeys = sort(collect(keys(azidata))) - +timestamps = intersect(sort(collect(keys(rangedata))), sort(collect(keys(azidata)))) # NAV data navdata = Dict{Int, Vector{Float64}}() @@ -55,31 +38,9 @@ for row in navfile # round(Int, 1000 * parse(s[1])) = 1531153292381 navdata[id] = parse.(s) end -# navdata[first(keys(navdata))] - navkeys = sort(collect(keys(navdata))) - -# latency is crazy - yep keys are timestamps # NAV colums are X,Y = 7,8 -# lat,long = 9,10 -# e.g. -# 10-element Array{Float64,1}: -# 1.53115e9 -# -6.33978 -# 0.449772 -# 129.62 -# 0.650937 -# 1.156 -# 51.9071 -# -33.1335 -# 42.3582 -# -71.087 - - - - - - +# lat,long = 9,10 X = Float64[] Y = Float64[] @@ -95,27 +56,16 @@ end # pl = Gadfly.plot(navdf, x=:x, y=:y, Geom.path()) # Gadfly.draw(Gadfly.PNG("/tmp/navss.png", 30cm, 20cm),pl) - interp_x = LinearInterpolation(navkeys, X) interp_y = LinearInterpolation(navkeys, Y) -interp_x(sum(navkeys[1:2])/2) - -navkeys[1] -epochs[1] - -interp_x(epochs[1]) - ## SELECT SEGMENT OF DATA TO WORK WITH ppbrDict = Dict{Int, Pose2Point2BearingRange}() - -epochs = rangekeys # [51:60] +epochs = timestamps # [51:60] GPS = Dict{Int, Vector{Float64}}() -# almost there -- be right back. -for ep in epochs - x, y = interp_x(ep), interp_y(ep) - GPS[ep] = [x;y] +@showprogress for ep in epochs + GPS[ep] = [interp_x(ep); interp_y(ep)] rangepts = rangedata[ep][:] rangeprob = kde!(rangepts) azipts = azidata[ep][:,1] @@ -125,12 +75,9 @@ for ep in epochs end -GPS_X[7:8] -GPS_Y[7:8] - +GPS[timestamps[1]] ## build the factor graph - fg = initfg() # Add a central beacon From ba093b42d668903bd921b487207ec69d863330fd Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 9 Sep 2018 11:27:52 -0500 Subject: [PATCH 23/47] Working session --- examples/marine/auv/Sandshark/Sandshark_2.jl | 38 ++++++++++++++------ 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index ca0d2006c..d87ceae02 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -2,6 +2,7 @@ using Caesar, RoME, KernelDensityEstimate using Interpolations +using Distributions using RoMEPlotting using Gadfly, DataFrames @@ -41,13 +42,17 @@ end navkeys = sort(collect(keys(navdata))) # NAV colums are X,Y = 7,8 # lat,long = 9,10 +# time,pitch,roll,yaw,speed,internal_x,internal_y,internal_lat,internal_long X = Float64[] Y = Float64[] +yaw = Float64[] for id in navkeys + push!(yaw, getindex(navdata[id],4)) push!(X, getindex(navdata[id],7)) push!(Y, getindex(navdata[id],8)) end + # navdf = DataFrame( # ts = navkeys - navkeys[1], # x = X, @@ -58,44 +63,57 @@ end interp_x = LinearInterpolation(navkeys, X) interp_y = LinearInterpolation(navkeys, Y) +interp_yaw = LinearInterpolation(navkeys, yaw) ## SELECT SEGMENT OF DATA TO WORK WITH ppbrDict = Dict{Int, Pose2Point2BearingRange}() +odoDict = Dict{Int, Pose2Pose2}() -epochs = timestamps # [51:60] -GPS = Dict{Int, Vector{Float64}}() +epochs = timestamps[51:60] +NAV = Dict{Int, Vector{Float64}}() +lastepoch = 0 @showprogress for ep in epochs - GPS[ep] = [interp_x(ep); interp_y(ep)] + if lastepoch != 0 + NAV[ep] = [ + interp_x(ep) - interp_x(lastepoch); + interp_y(ep) - interp_y(lastepoch); + TransformUtils.wrapRad(deg2rad(interp_yaw(ep) - interp_yaw(lastepoch)))] # Bearing? + odoDict[ep] = Pose2Pose2(MvNormal(NAV[ep], diagm([0.1;0.1;0.005].^2))) + end rangepts = rangedata[ep][:] rangeprob = kde!(rangepts) azipts = azidata[ep][:,1] aziprob = kde!(azipts) # prep the factor functions ppbrDict[ep] = Pose2Point2BearingRange(aziprob, rangeprob) -end + lastepoch = ep +end -GPS[timestamps[1]] ## build the factor graph fg = initfg() # Add a central beacon addNode!(fg, :l1, Point2) -# addFactor() index = 0 -for ep in epochs +epoch_slice = epochs +for ep in epoch_slice # Correct variable type Pose2? curvar = Symbol("x$index") addNode!(fg, curvar, Pose2) - addFactor!(fg, [curvar; :l1], ppbrDict[ep]) # that makes sense - gotcha -- I was on odo sorry + # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) # that makes sense - gotcha -- I was on odo sorry + if ep != epoch_slice[1] + addFactor!(fg, [Symbol("x$(index-1)") curvar], odoDict[ep]) + else + # add a prior to the first pose location (a "GPS" prior) + addFactor!(fg, [curvar], Prior(MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.005].^2))) + end index+=1 end writeGraphPdf(fg) - - batchSolve!(fg) From 1aa0c7fd93a3d1e160551300507e962723f9963c Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 9 Sep 2018 21:24:46 -0500 Subject: [PATCH 24/47] WIP on Sandshark - currently still needs some work --- examples/marine/auv/Sandshark/Sandshark_2.jl | 81 +++++++++++++++----- 1 file changed, 60 insertions(+), 21 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index d87ceae02..8f53c5c20 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -1,6 +1,6 @@ # new Sandshark example -using Caesar, RoME, KernelDensityEstimate +using Caesar, RoME, KernelDensityEstimate, IncrementalInference using Interpolations using Distributions @@ -32,7 +32,7 @@ timestamps = intersect(sort(collect(keys(rangedata))), sort(collect(keys(azidata # NAV data navdata = Dict{Int, Vector{Float64}}() -navfile = readdlm(joinpath(datadir, "nav_data.csv")) +navfile = readdlm(joinpath(datadir, "new_nav_data2.csv")) for row in navfile s = split(row, ",") id = round(Int, 1000*parse(s[1])) @@ -42,17 +42,27 @@ end navkeys = sort(collect(keys(navdata))) # NAV colums are X,Y = 7,8 # lat,long = 9,10 -# time,pitch,roll,yaw,speed,internal_x,internal_y,internal_lat,internal_long +# time,pitch,roll,yaw,speed,[Something], internal_x,internal_y,internal_lat,internal_long + +# heading_to_pi(val) +# ret = val; +# ret = 2*pi - ret; +# ret = ret - 1.5*pi; +# if ret < -pi +# ret = ret + 2*pi; +# end +# end X = Float64[] Y = Float64[] yaw = Float64[] for id in navkeys - push!(yaw, getindex(navdata[id],4)) + push!(yaw, getindex(navdata[id],10)) push!(X, getindex(navdata[id],7)) push!(Y, getindex(navdata[id],8)) end +# tssubset = navkeys[51:60] # navdf = DataFrame( # ts = navkeys - navkeys[1], # x = X, @@ -60,7 +70,7 @@ end # ) # pl = Gadfly.plot(navdf, x=:x, y=:y, Geom.path()) # Gadfly.draw(Gadfly.PNG("/tmp/navss.png", 30cm, 20cm),pl) - +# interp_x = LinearInterpolation(navkeys, X) interp_y = LinearInterpolation(navkeys, Y) interp_yaw = LinearInterpolation(navkeys, yaw) @@ -72,12 +82,16 @@ odoDict = Dict{Int, Pose2Pose2}() epochs = timestamps[51:60] NAV = Dict{Int, Vector{Float64}}() lastepoch = 0 -@showprogress for ep in epochs +for ep in epochs if lastepoch != 0 - NAV[ep] = [ + # @show interp_yaw(ep) + deltaAng = interp_yaw(ep) - interp_yaw(lastepoch) + odoVec = [ interp_x(ep) - interp_x(lastepoch); interp_y(ep) - interp_y(lastepoch); - TransformUtils.wrapRad(deg2rad(interp_yaw(ep) - interp_yaw(lastepoch)))] # Bearing? + TransformUtils.wrapRad(deltaAng)] + NAV[ep] = odoVec + println("$(odoVec[1]), $(odoVec[2]), $(odoVec[3])") odoDict[ep] = Pose2Pose2(MvNormal(NAV[ep], diagm([0.1;0.1;0.005].^2))) end rangepts = rangedata[ep][:] @@ -90,38 +104,63 @@ lastepoch = 0 lastepoch = ep end +# Sanity check.... +for ep in epochs + x = interp_x(ep) + y = interp_y(ep) + yaw = interp_yaw(ep) + println("$ep, $x, $y, $yaw") +end + ## build the factor graph fg = initfg() # Add a central beacon -addNode!(fg, :l1, Point2) +# addNode!(fg, :l1, Point2) +# Pinger location is x=16, y=0.6 + index = 0 epoch_slice = epochs for ep in epoch_slice - # Correct variable type Pose2? - curvar = Symbol("x$index") - addNode!(fg, curvar, Pose2) - # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) # that makes sense - gotcha -- I was on odo sorry - if ep != epoch_slice[1] - addFactor!(fg, [Symbol("x$(index-1)") curvar], odoDict[ep]) - else + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + if ep != epoch_slice[1] + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + else # add a prior to the first pose location (a "GPS" prior) - addFactor!(fg, [curvar], Prior(MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.005].^2))) - end - index+=1 + println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") + addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.05].^2)) )) + end + index+=1 end - writeGraphPdf(fg) -batchSolve!(fg) +IIF.batchSolve!(fg) #, N=100 +# see pictures +drawPoses(fg, spscale=0.75) +drawPosesLandms(fg, spscale=0.75) +## Debugging strange headings +posePts = get2DPoseMeans(fg) +## Debugging landmark bearing range +# ppbrDict[epoch_slice[1]] +getSample(ppbrDict[epoch_slice[1]],100) + + +addNode!(fg, :l1, Point2) +addFactor!(fg, [:x0; :l1], ppbrDict[epoch_slice[1]]) +ls(fg, :l1) +pts = IIF.approxConv(fg, :x0l1f1, :l1) +fct = getData(getVert(fg, :x0l1f1, nt=:fnc)) +fct.fnc.zDim From 2c37c2da764c97aad2e377b7f9dcd5418143e778 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 9 Sep 2018 21:28:49 -0500 Subject: [PATCH 25/47] Adding prior for pinger --- examples/marine/auv/Sandshark/Sandshark_2.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index 8f53c5c20..06d669f26 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -119,6 +119,7 @@ fg = initfg() # Add a central beacon # addNode!(fg, :l1, Point2) # Pinger location is x=16, y=0.6 +# addFactor!(fg, [curvar], IIF.Prior( MvNormal([16;0.6], diagm([0.1;0.1].^2)) )) index = 0 epoch_slice = epochs From af44ac90af79b15af45736be5dab3bfb65ed81e7 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Mon, 10 Sep 2018 07:57:15 -0500 Subject: [PATCH 26/47] New datafile --- examples/marine/auv/Sandshark/Sandshark_2.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index 06d669f26..5c96c694a 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -32,7 +32,7 @@ timestamps = intersect(sort(collect(keys(rangedata))), sort(collect(keys(azidata # NAV data navdata = Dict{Int, Vector{Float64}}() -navfile = readdlm(joinpath(datadir, "new_nav_data2.csv")) +navfile = readdlm(joinpath(datadir, "nav_data2.csv")) for row in navfile s = split(row, ",") id = round(Int, 1000*parse(s[1])) @@ -42,7 +42,7 @@ end navkeys = sort(collect(keys(navdata))) # NAV colums are X,Y = 7,8 # lat,long = 9,10 -# time,pitch,roll,yaw,speed,[Something], internal_x,internal_y,internal_lat,internal_long +# time,pitch,roll,yaw,speed,[Something], internal_x,internal_y,internal_lat,internal_long, yaw_rad # heading_to_pi(val) # ret = val; @@ -57,7 +57,7 @@ X = Float64[] Y = Float64[] yaw = Float64[] for id in navkeys - push!(yaw, getindex(navdata[id],10)) + push!(yaw, getindex(navdata[id],11)) push!(X, getindex(navdata[id],7)) push!(Y, getindex(navdata[id],8)) end From 100f584f2cb89003b9b0c6d098a1bdb2a0952bda Mon Sep 17 00:00:00 2001 From: GearsAD Date: Mon, 10 Sep 2018 18:42:31 -0500 Subject: [PATCH 27/47] Stil WIP, now has functions for 3 graph types --- examples/marine/auv/Sandshark/Sandshark_2.jl | 110 +++++++++++++++---- 1 file changed, 87 insertions(+), 23 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index 5c96c694a..2b41189fd 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -1,4 +1,6 @@ # new Sandshark example +# add more julia processes +nprocs() < 7 ? addprocs(8-nprocs()) : nothing using Caesar, RoME, KernelDensityEstimate, IncrementalInference using Interpolations @@ -79,6 +81,7 @@ interp_yaw = LinearInterpolation(navkeys, yaw) ppbrDict = Dict{Int, Pose2Point2BearingRange}() odoDict = Dict{Int, Pose2Pose2}() +# We have 261 timestamps epochs = timestamps[51:60] NAV = Dict{Int, Vector{Float64}}() lastepoch = 0 @@ -112,41 +115,102 @@ for ep in epochs println("$ep, $x, $y, $yaw") end +## build the factor graph +function buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw)::IncrementalInference.FactorGraph + fg = initfg() + + # Don't care about beacon right now + # addNode!(fg, :l1, Point2) + # Pinger location is x=16, y=0.6 + # addFactor!(fg, [curvar], IIF.Prior( MvNormal([16;0.6], diagm([0.1;0.1].^2)) )) + + index = 0 + for ep in epochs + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + if ep != epochs[1] + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + else + # add a prior to the first pose location (a "GPS" prior) + println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") + addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.05].^2)) )) + end + index+=1 + end + + return fg +end + +## build the factor graph +function buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw)::IncrementalInference.FactorGraph + fg = initfg() + + # Add a central beacon with a prior + addNode!(fg, :l1, Point2) + # Pinger location is x=16, y=0.6 + addFactor!(fg, [:l1], IIF.Prior( MvNormal([16;0.6], diagm([0.1;0.1].^2)) )) + + index = 0 + for ep in epochs + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + if ep != epochs[1] + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + else + # add a prior to the first pose location (a "GPS" prior) + println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") + addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.05].^2)) )) + end + index+=1 + end + + return fg +end ## build the factor graph -fg = initfg() - -# Add a central beacon -# addNode!(fg, :l1, Point2) -# Pinger location is x=16, y=0.6 -# addFactor!(fg, [curvar], IIF.Prior( MvNormal([16;0.6], diagm([0.1;0.1].^2)) )) - -index = 0 -epoch_slice = epochs -for ep in epoch_slice - curvar = Symbol("x$index") - addNode!(fg, curvar, Pose2) - # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) - if ep != epoch_slice[1] - addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) - else - # add a prior to the first pose location (a "GPS" prior) - println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") - addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.05].^2)) )) +function buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw)::IncrementalInference.FactorGraph + fg = initfg() + + # Add a central beacon, no prior + addNode!(fg, :l1, Point2) + + index = 0 + for ep in epochs + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + if ep != epochs[1] + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + end + index+=1 end - index+=1 + # add a prior to the first pose location (a "GPS" prior) + println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") + addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(epochs[1]);interp_y(epochs[1]);interp_yaw(epochs[1])], diagm([0.1;0.1;0.05].^2)) )) + + return fg end +# Various graph options - choose one +fg = buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) +# fg = buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) +# fg = buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) + +# Solvery! Roll dice for solvery check writeGraphPdf(fg) IIF.batchSolve!(fg) #, N=100 -# see pictures +# Roll again for inspiration check drawPoses(fg, spscale=0.75) -drawPosesLandms(fg, spscale=0.75) +drawPosesLandms(fg, spscale=0.75) #Means so we don't run into MM == Union() || Dict{} in +# You rolled 20! +# Roll agin for debug check ## Debugging strange headings posePts = get2DPoseMeans(fg) - +landPts = get2DLandmMeans(fg) ## Debugging landmark bearing range # ppbrDict[epoch_slice[1]] From 9f11ca60a32aaf47e4668e9c75b429cad6d79994 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Tue, 11 Sep 2018 22:23:25 -0500 Subject: [PATCH 28/47] Dehann fixed transforms with wizardry --- examples/marine/auv/Sandshark/Sandshark_2.jl | 141 +++++++++++++++++-- 1 file changed, 126 insertions(+), 15 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index 2b41189fd..87246fb65 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -10,6 +10,8 @@ using RoMEPlotting using Gadfly, DataFrames using ProgressMeter +const TU = TransformUtils + datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") matcheddir = joinpath(datadir, "matchedfilter", "particles") beamdir = joinpath(datadir, "beamformer", "particles") @@ -32,9 +34,10 @@ rangedata = loaddircsvs(matcheddir) azidata = loaddircsvs(beamdir) timestamps = intersect(sort(collect(keys(rangedata))), sort(collect(keys(azidata)))) + # NAV data navdata = Dict{Int, Vector{Float64}}() -navfile = readdlm(joinpath(datadir, "nav_data2.csv")) +navfile = readdlm(joinpath(datadir, "nav_data.csv")) for row in navfile s = split(row, ",") id = round(Int, 1000*parse(s[1])) @@ -44,7 +47,7 @@ end navkeys = sort(collect(keys(navdata))) # NAV colums are X,Y = 7,8 # lat,long = 9,10 -# time,pitch,roll,yaw,speed,[Something], internal_x,internal_y,internal_lat,internal_long, yaw_rad +# time,pitch,roll,heading,speed,[Something], internal_x,internal_y,internal_lat,internal_long, yaw_rad # heading_to_pi(val) # ret = val; @@ -55,15 +58,30 @@ navkeys = sort(collect(keys(navdata))) # end # end +# GET Y = north, X = East, Heading along +Y clockwise [0,360)] +east = Float64[] +north = Float64[] +heading = Float64[] + +# WANT X = North, Y = West, Yaw is right and rule from +X (0) towards +Y pi/2, [-pi,pi) +# so the drawPoses picture will look flipped from Nicks picture +# remember theta = atan2(y,x) # this is right hand rule + X = Float64[] Y = Float64[] yaw = Float64[] for id in navkeys - push!(yaw, getindex(navdata[id],11)) - push!(X, getindex(navdata[id],7)) - push!(Y, getindex(navdata[id],8)) + push!(east, getindex(navdata[id],7)) # x-column csv + push!(north, getindex(navdata[id],8)) # y-column csv + push!(heading, getindex(navdata[id],4)) + # push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4)))) + + push!(X, getindex(navdata[id],8) ) + push!(Y, -getindex(navdata[id],7) ) + push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4))) ) # rotation about +Z end + # tssubset = navkeys[51:60] # navdf = DataFrame( # ts = navkeys - navkeys[1], @@ -82,19 +100,26 @@ ppbrDict = Dict{Int, Pose2Point2BearingRange}() odoDict = Dict{Int, Pose2Pose2}() # We have 261 timestamps -epochs = timestamps[51:60] +epochs = timestamps[51:100] NAV = Dict{Int, Vector{Float64}}() lastepoch = 0 for ep in epochs if lastepoch != 0 # @show interp_yaw(ep) deltaAng = interp_yaw(ep) - interp_yaw(lastepoch) - odoVec = [ - interp_x(ep) - interp_x(lastepoch); - interp_y(ep) - interp_y(lastepoch); - TransformUtils.wrapRad(deltaAng)] - NAV[ep] = odoVec - println("$(odoVec[1]), $(odoVec[2]), $(odoVec[3])") + # wDXij = + # [interp_x(ep) - interp_x(lastepoch); + # interp_y(ep) - interp_y(lastepoch)] + wXi = TU.SE2([interp_x(lastepoch);interp_y(lastepoch);interp_yaw(lastepoch)]) + wXj = TU.SE2([interp_x(ep);interp_y(ep);interp_yaw(ep)]) + iDXj = se2vee(wXi\wXj) + # odoVec = iDXj + # odoVec = [ + # mag; + # mag; + # TransformUtils.wrapRad(deltaAng)] + NAV[ep] = iDXj + println("$(iDXj[1]), $(iDXj[2]), $(iDXj[3])") odoDict[ep] = Pose2Pose2(MvNormal(NAV[ep], diagm([0.1;0.1;0.005].^2))) end rangepts = rangedata[ep][:] @@ -175,6 +200,8 @@ function buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, # Add a central beacon, no prior addNode!(fg, :l1, Point2) + # addFactor!(fg, [:l1], IIF.Prior( MvNormal([16;0.6], diagm([0.1;0.1].^2)) )) + setVal!(fg, :l1, zeros(2,100)) index = 0 for ep in epochs @@ -182,13 +209,13 @@ function buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, addNode!(fg, curvar, Pose2) addFactor!(fg, [curvar; :l1], ppbrDict[ep]) if ep != epochs[1] - addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep], autoinit=false) end index+=1 end # add a prior to the first pose location (a "GPS" prior) - println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") - addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(epochs[1]);interp_y(epochs[1]);interp_yaw(epochs[1])], diagm([0.1;0.1;0.05].^2)) )) + println("Adding a prior") + addFactor!(fg, [:x0], IIF.Prior( MvNormal([interp_x(epochs[1]);interp_y(epochs[1]);interp_yaw(epochs[1])], diagm([0.1;0.1;0.05].^2)) ), autoinit=false) return fg end @@ -200,6 +227,7 @@ fg = buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw # Solvery! Roll dice for solvery check writeGraphPdf(fg) +# ensureAllInitialized!(fg) IIF.batchSolve!(fg) #, N=100 # Roll again for inspiration check @@ -229,4 +257,87 @@ fct.fnc.zDim + + + + + + + + + + + +#### DEBUGGGGG==================== + +# timestamps[51] +# +# fg_demo = RoME.initfg() +# +# addNode!(fg_demo, :x1, Pose2) +# addFactor!(fg_demo, [:x1], IIF.Prior(MvNormal(zeros(3), 0.01^2*eye(3)))) +# +# ensureAllInitialized!(fg_demo) +# +# drawPoses(fg_demo) +# +# +# +# +# +# fg = RoME.initfg() +# +# addNode!(fg, :x51, Pose2) +# addFactor!(fg, [:x51], IIF.Prior(MvNormal([X[1259];Y[1259];yaw[1259]], 0.001^2*eye(3)))) +# +# addNode!(fg, :x52, Pose2) +# wXi = TU.SE2([X[1259];Y[1259];yaw[1259]]) +# wXj = TU.SE2([X[1269];Y[1269];yaw[1269]]) +# iDXj = se2vee(wXi\wXj) +# pp = Pose2Pose2(MvNormal([iDXj...], 0.001^2*eye(3)) ) +# addFactor!(fg, [:x51;:x52], pp) +# +# addNode!(fg, :x53, Pose2) +# wXi = TU.SE2([X[1269];Y[1269];yaw[1269]]) +# wXj = TU.SE2([X[1279];Y[1279];yaw[1279]]) +# iDXj = se2vee(wXi\wXj) +# pp = Pose2Pose2(MvNormal([iDXj...], 0.001^2*eye(3)) ) +# addFactor!(fg, [:x52;:x53], pp) +# +# addNode!(fg, :x54, Pose2) +# wXi = TU.SE2([X[1279];Y[1279];yaw[1279]]) +# wXj = TU.SE2([X[1289];Y[1289];yaw[1289]]) +# iDXj = se2vee(wXi\wXj) +# pp = Pose2Pose2(MvNormal([iDXj...], 0.001^2*eye(3)) ) +# addFactor!(fg, [:x53;:x54], pp) +# +# addNode!(fg, :x55, Pose2) +# wXi = TU.SE2([X[1289];Y[1289];yaw[1289]]) +# wXj = TU.SE2([X[1299];Y[1299];yaw[1299]]) +# iDXj = se2vee(wXi\wXj) +# pp = Pose2Pose2(MvNormal([iDXj...], 0.001^2*eye(3)) ) +# addFactor!(fg, [:x54;:x55], pp) +# +# ensureAllInitialized!(fg) +# +# # addNode!(fg, :x56, Pose2) +# # addNode!(fg, :x57, Pose2) +# # addNode!(fg, :x58, Pose2) +# # addNode!(fg, :x59, Pose2) +# # addNode!(fg, :x60, Pose2) +# +# drawPoses(fg, spscale=0.1) +# +# yaw[1259]*180.0/pi +# navdata[navkeys[1259]][4] +# yaw[1259] +# +# +# 0 +# +# Gadfly.plot(x=X, x=Y, Geom.path) + + + + # From 89899e9631602fa193deaf6e55bd81231666804d Mon Sep 17 00:00:00 2001 From: GearsAD Date: Wed, 12 Sep 2018 00:00:16 -0500 Subject: [PATCH 29/47] Plot magic --- examples/marine/auv/Sandshark/Sandshark_2.jl | 119 ++++++++++++++++++- 1 file changed, 114 insertions(+), 5 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index 87246fb65..f3515ee5b 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -100,7 +100,7 @@ ppbrDict = Dict{Int, Pose2Point2BearingRange}() odoDict = Dict{Int, Pose2Pose2}() # We have 261 timestamps -epochs = timestamps[51:100] +epochs = timestamps[50:2:261] NAV = Dict{Int, Vector{Float64}}() lastepoch = 0 for ep in epochs @@ -174,7 +174,7 @@ function buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, in # Add a central beacon with a prior addNode!(fg, :l1, Point2) # Pinger location is x=16, y=0.6 - addFactor!(fg, [:l1], IIF.Prior( MvNormal([16;0.6], diagm([0.1;0.1].^2)) )) + addFactor!(fg, [:l1], IIF.Prior( MvNormal([0.6; -16], diagm([0.1;0.1].^2)) )) index = 0 for ep in epochs @@ -220,9 +220,57 @@ function buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, return fg end +function layerBeamPatternRose(bear::BallTreeDensity; scale::Float64=1.0, c=colorant"magenta", wRr=TU.R(0.0),wTRr=zeros(2)) + tp = reshape([0:0.01:(2pi);], 1, :) + belp = scale*bear(tp) + Gadfly.plot(x=tp, y=belp, Geom.path) + belRose = zeros(2, length(tp)) + belRose[1,:] = belp + + idx = 0 + for rRc in TU.R.(tp) + idx += 1 + belRose[:,idx] = wRr*rRc*belRose[:,idx] + wTRr + end + + Gadfly.layer(x=belRose[1,:], y=belRose[2,:], Geom.path, Theme(default_color=c)) +end + + +# pl = drawPoses(fg, spscale=2.5) +function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y) + PLL = [] + xx, = ls(fg) + idx = 0 + for ep in epochs + idx += 1 + theta = getKDEMax(getVertKDE(fg, xx[idx])) + wRr = TU.R(theta[3]) + pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=wRr, wTRr=theta[1:2], scale=5.0) + push!(PLL, pll) + end + + pllandmarks = drawPosesLandms(fg, spscale=2.5) + # Add odo + navdf = DataFrame( + ts = navkeys, + x = X, + y = Y + ) + # pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) + push!(pllandmarks.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"red"))[1]) + # Gadfly.plot(pl.layers) + + push!(PLL, Coord.Cartesian(xmin=-150.0,xmax=10.0,ymin=-120.0,ymax=20.0)) + pla = plot([PLL;pllandmarks.layers; ]...) + return pla +end + + + # Various graph options - choose one -fg = buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) -# fg = buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) +# fg = buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) +fg = buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) # fg = buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) # Solvery! Roll dice for solvery check @@ -231,7 +279,7 @@ writeGraphPdf(fg) IIF.batchSolve!(fg) #, N=100 # Roll again for inspiration check -drawPoses(fg, spscale=0.75) +pl = drawPoses(fg, spscale=2.75) drawPosesLandms(fg, spscale=0.75) #Means so we don't run into MM == Union() || Dict{} in # You rolled 20! @@ -258,14 +306,75 @@ fct.fnc.zDim +## PLOT BEAM PATTERNS + +Gadfly.push_theme(:default) +pla = drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y) +Gadfly.draw(PDF("sandshark-beacon.pdf", 12cm, 15cm), pla) +Gadfly.draw(PNG("sandshark-beacon.png", 12cm, 15cm), pla) + + +# dev +# ep = epochs[1] +# # azidata[ep][:,1] +# pll = layerBeamPatternRose(ppbrDict[ep].bearing) +# Gadfly.plot(pll...) +# +# pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=TU.R(pi/2)) +# Gadfly.plot(pll...) + + +diff(epochs[[end;1]]) + + + +## GADFLY EXAMPLE + + +PL = [] + +push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"red"))[1]) +push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"green"))[1]) +push!(PL, Coord.Cartesian(xmin=-1.0,xmax=11, ymin=-5.0, ymax=5.0)) +push!(PL, Guide.ylabel("test")) +push!(PL, Guide.ylabel("test")) +pl = drawPoses(fg) +push!(pl.layers, ) + + + +pl = drawPoses(fg) +pl = Gadfly.plot(pl.layers...) + + +pl = Gadfly.plot(PL...) +@show fieldnames(pl) +push!(pl.layers, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"magenta"))[1] ) +pl +# vstasck, hstack +Gadfly.push_theme(:default) +# SVG, PDF, PNG +Gadfly.draw(PDF("/tmp/testfig.pdf", 12cm, 8cm), pl) +run(`evince /tmp/testfig.pdf`) +### PATH + Poses +pl = drawPosesLandms(fg, spscale=2.5) +# Add odo +navdf = DataFrame( + ts = navkeys, + x = X, + y = Y +) +# pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) +push!(pl.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path())[1]) +Gadfly.plot(pl.layers) #### DEBUGGGGG==================== From bc218de406a235ac0ee9298b1e9d0eaa60d053c4 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 13 Sep 2018 16:09:28 -0400 Subject: [PATCH 30/47] wip --- .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 173 ++++++++++++++++-- examples/wheeled/racecar/racecarUtils.jl | 4 +- .../wheeled/racecar/visualizationUtils.jl | 96 ++++++++++ examples/wheeled/victoriapark_stepbystep.jl | 10 +- 4 files changed, 260 insertions(+), 23 deletions(-) diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index 7a0b9d910..363eadce4 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -1,7 +1,7 @@ # Local compute version # add more julia processes -# nprocs() < 5 ? addprocs(5-nprocs()) : nothing +nprocs() < 5 ? addprocs(5-nprocs()) : nothing using Caesar, RoME, Distributions using YAML, JLD, HDF5 @@ -43,24 +43,27 @@ Rz = RotZ(-pi/2) bTc= LinearMap(Rz) ∘ LinearMap(Rx) -# datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" +datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" # datafolder = ENV["HOME"]*"/data/racecar/labrun2/" # datafolder = ENV["HOME"]*"/data/racecar/labrun3/" # datafolder = ENV["HOME"]*"/data/racecar/labrun5/" # datafolder = ENV["HOME"]*"/data/racecar/labrun6/" -datafolder = ENV["HOME"]*"/data/racecar/labfull/" +# datafolder = ENV["HOME"]*"/data/racecar/labfull/" imgfolder = "images" # Figure export folder currdirtime = now() # currdirtime = "2018-08-14T00:52:01.534" +# currdirtime = "2018-09-10T09:22:00.922" resultsdir = joinpath(ENV["HOME"], "Pictures", "racecarimgs") imgdir = joinpath(resultsdir, "$(currdirtime)") + mkdir(imgdir) mkdir(imgdir*"/tags") +mkdir(imgdir*"/images") -camidxs = 0:5:1765 +camidxs = 175:5:370 #0:5:1765 fid = open(imgdir*"/readme.txt", "w") println(fid, datafolder) @@ -98,7 +101,7 @@ pssym = Symbol("x$psid") addNode!(fg, pssym, DynPose2(ut=0)) # addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)))) addFactor!(fg, [pssym], DynPose2VelocityPrior(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)), - MvNormal(zeros(2),diagm([0.05;0.05].^2)))) + MvNormal(zeros(2),diagm([0.1;0.05].^2)))) addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2, fcttype=DynPose2Pose2) @@ -120,13 +123,16 @@ prev_psid = 0 # add other positions maxlen = (length(tag_bag)-1) # psid = 5 -for psid in 1:1:50 #maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen +for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen @show psym = Symbol("x$psid") addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=VelPose2VelPose2, fcttype=DynPose2Pose2, autoinit=true) # writeGraphPdf(fg) - if psid % 1000 == 0 || psid == maxlen + + + if psid % 20 == 0 || psid == maxlen + IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym)_presolve.jld") tree = wipeBuildNewTree!(fg, drawpdf=true) - # inferOverTree!(fg,tree, N=N) + inferOverTree!(fg,tree, N=N) end ## save factor graph for later testing and evaluation @@ -143,25 +149,153 @@ for psid in 1:1:50 #maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen prev_psid = psid end -# save factor graph for later testing and evaluation IIF.savejld(fg, file=imgdir*"/racecar_fg_final.jld") +# fg, = loadjld(file=imgdir*"/racecar_fg_final.jld") -tree = wipeBuildNewTree!(fg, drawpdf=true) -# # @async run(`evince bt.pdf`) -inferOverTreeR!(fg,tree, N=N) -# inferOverTree!(fg,tree, N=N) +# save factor graph for later testing and evaluation +batchSolve!(fg, drawpdf=true, N=N) + +IIF.savejld(fg, file=imgdir*"/racecar_fg_final_resolve.jld") +# fgr, = loadjld(file=imgdir*"/racecar_fg_final_resolve.jld") -# pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) #,xmin=-3,xmax=6,ymin=-5,ymax=2); -# Gadfly.draw(PNG(joinpath(imgdir,"final.png"),15cm, 10cm),pl) + +#,xmin=-3,xmax=6,ymin=-5,ymax=2); +Gadfly.push_theme(:default) +pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean, to=3) +Gadfly.draw(SVG(joinpath(imgdir,"images","final.svg"),15cm, 10cm),pl) # pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); # Gadfly.draw(PNG(joinpath(imgdir,"hist_final.png"),15cm, 10cm),pl) +drawPoses(fg, spscale=1.0) +drawPosesLandms(fg, spscale=0.5) + + +sym = :x2 +ls(fg, sym) +pl = plotLocalProduct(fg, sym, dims=[1]) +Gadfly.draw(SVG(joinpath(imgdir,"images","localp_$sym.svg"),15cm, 3*10cm),pl) + + +stuff = IIF.localProduct(fg, :l0) + + +P = *(stuff[3][1]) + +plotKDE([P;stuff[3][1]], c=["red";["cyan" for j in 1:length(stuff[3][1])]]) + +getVal(fg, :l1) + + + +## + + + + +genGifLandm(fg, :l1, IMGS) + + + +fsym = :x0l0f1 +getData(getVert(fg, fsym,nt=:fnc)).fnc.usrfnc!.Zpose.z.μ + +XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) +@show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) +bear= TU.wrapRad(atan2(-xyt[2],xyt[1]) - XX[3]) + + + +im = imageFactor(fg, :x0l1f1, IMGS[1], cfg) +BRIMGS = Dict{Symbol,Dict{Symbol, Any}}() +BRIMGS[:x0] = Dict{Symbol,Any}() +BRIMGS[:x0][:l1] = im +BIM = drawAllBearingImgs(fg, IMGS, cfg) +BIM[:x0][:l1] -#0 +sym = :l1 + + +imageFactor(fg, :x3l0f1, IMGS[4], cfg) +imageFactor(fg, :x3l1f1, IMGS[4], cfg) +imageFactor(fg, :x3l2f1, IMGS[4], cfg) + + + +0 + +#j +# # using KernelDensityEstimate +# # import IncrementalInference: localProduct, proposalbeliefs!, productbelief,findRelatedFromPotential +# function localProduct(fgl::FactorGraph, +# sym::Symbol; +# N::Int=100, +# dbg::Bool=false, +# api::DataLayerAPI=IncrementalInference.dlapi ) +# # TODO -- converge this function with predictbelief for this node +# # TODO -- update to use getVertId +# destvertid = fgl.IDs[sym] #destvert.index +# dens = Array{BallTreeDensity,1}() +# partials = Dict{Int, Vector{BallTreeDensity}}() +# lb = String[] +# fcts = Vector{Graphs.ExVertex}() +# cf = ls(fgl, sym, api=api) +# for f in cf +# vert = getVert(fgl,f, nt=:fnc, api=api) +# push!(fcts, vert) +# push!(lb, vert.label) +# end +# +# # get proposal beliefs +# proposalbeliefs!(fgl, destvertid, fcts, dens, partials, N=N, dbg=dbg) +# # @show length(dens), length(partials) +# +# # take the product +# pGM = productbelief(fgl, destvertid, dens, partials, N, dbg=dbg ) +# pp = kde!(pGM) +# +# return pp, dens, partials, lb +# end +# +# +# function proposalbeliefs!(fgl::FactorGraph, +# destvertid::Int, +# factors::Vector{Graphs.ExVertex}, +# dens::Vector{BallTreeDensity}, +# partials::Dict{Int, Vector{BallTreeDensity}}; +# N::Int=100, +# dbg::Bool=false ) +# # +# for fct in factors +# data = getData(fct) +# p = findRelatedFromPotential(fgl, fct, destvertid, N, dbg) +# if data.fnc.partial # partial density +# pardims = data.fnc.usrfnc!.partial +# for dimnum in pardims +# if haskey(partials, dimnum) +# push!(partials[dimnum], marginal(p,[dimnum])) +# else +# partials[dimnum] = BallTreeDensity[marginal(p,[dimnum])] +# end +# end +# else # full density +# push!(dens, p) +# end +# end +# nothing +# end +# + + + + + + + +0 # # ls(fg, :l7) # @@ -200,6 +334,13 @@ inferOverTreeR!(fg,tree, N=N) + + + + + + + 0 diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index c98ab51bc..edd9385b3 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -75,8 +75,8 @@ function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odot addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.4;0.1;0.4].^2))), autoinit=autoinit) elseif odotype == VelPose2VelPose2 addNode!(fg, new_pssym, DynPose2(ut=round(Int, 200_000*(new_psid)))) - addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.4;0.07;0.1].^2)), - MvNormal(zeros(2),diagm([0.2;0.2].^2))), autoinit=autoinit) + addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.4;0.4;0.2].^2)), + MvNormal(zeros(2),diagm([0.4;0.1].^2))), autoinit=autoinit) end addApriltags!(fg, new_pssym, pose_tag_bag, lmtype=lmtype, fcttype=fcttype, DAerrors=DAerrors) diff --git a/examples/wheeled/racecar/visualizationUtils.jl b/examples/wheeled/racecar/visualizationUtils.jl index ea712ffa4..1b4536dda 100644 --- a/examples/wheeled/racecar/visualizationUtils.jl +++ b/examples/wheeled/racecar/visualizationUtils.jl @@ -123,5 +123,101 @@ end +# tan(bear) = u/f + + + + + +# L = X + DX => X\L = DX + +function imageFactor(fg, fsym, im, cfg) + img = Gray.(im) + + zmeas = getData(getVert(fg, fsym,nt=:fnc)).fnc.usrfnc!.Zpose.z.μ + + # @show IIF.lsf(fg, fsym) + XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) + @show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) + bear= TU.wrapRad(atan2(-xyt[2],xyt[1]) - XX[3]) + # bear = 0.0 + focal= cfg[:intrinsics][:fx] + cu = cfg[:intrinsics][:cx] + # + @show bu = tan(bear)*focal + cu + intens = pdf.(Distributions.Normal(bu,30.0), collect(1:size(img,2))) + intens ./= maximum(intens) + + zbear= TU.wrapRad(atan2(-zmeas[2],zmeas[1]) - 0.0) + + @show zbu = tan(zbear)*focal + cu + zintens = pdf.(Distributions.Normal(zbu,30.0), collect(1:size(img,2))) + zintens ./= maximum(zintens) + + + for i in 1:round(Int,0.3*size(img, 1)) + img[i, :] .*= intens + end + for i in round(Int,0.3*size(img, 1)):size(img,1) + img[i, :] .*= zintens + end + + img +end + + +function drawAllBearingImgs(fg, IMGS, cfg) + BRIMGS = Dict{Symbol,Dict{Symbol, Any}}() + XX, LL = ls(fg) + + idx = 0 + for x in XX + idx += 1 + BRIMGS[x] = Dict{Symbol,Any}() + for l in LL + try + fsym = Symbol("$(x)$(l)f1") + im = imageFactor(fg, fsym, IMGS[idx], cfg) + BRIMGS[x][l] = im + catch Er + # + end + end + end + return BRIMGS +end + + +function genGifLandm(fg, sym, IMGS; show=true, delay=50) + BIM = drawAllBearingImgs(fg, IMGS, cfg) + TEMP = [] + for ss in split.(string.(ls(fg,sym)),string(sym)) + push!(TEMP, BIM[Symbol(ss[1])][sym]) + end + + # mkdir("/tmp/animate_tags") + # Base.rm("/tmp/animate_tags") + run(`rm -fr /tmp/animate_tags`) + mkdir("/tmp/animate_tags") + for i in 1:length(TEMP) + save("/tmp/animate_tags/im$(i).png", TEMP[i]) + end + + run(`convert -delay $(delay) /tmp/animate_tags/im*.png $(sym).gif`) + + !show ? nothing : @async run(`eog $(sym).gif`) + return "$(sym).gif" +end + + +function getAllLandmGifs(fg, IMGS; show=false) + info("Dropping gifs here $(Base.pwd())") + for l in ls(fg)[2] + genGifLandm(fg, l, IMGS, show=show) + end + nothing +end + + # diff --git a/examples/wheeled/victoriapark_stepbystep.jl b/examples/wheeled/victoriapark_stepbystep.jl index 9a3956342..5d2faa027 100644 --- a/examples/wheeled/victoriapark_stepbystep.jl +++ b/examples/wheeled/victoriapark_stepbystep.jl @@ -23,10 +23,10 @@ function evalLikelihood(fg::FactorGraph, sym::Symbol, points::Array{Float64,2}) evaluateDualTree(p, (points)) end -function batchSolve(fgl) - tree = prepBatchTree!(fgl, drawpdf=true); - @time inferOverTree!(fgl,tree, N=100); -end +# function batchSolve(fgl) +# tree = prepBatchTree!(fgl, drawpdf=true); +# @time inferOverTree!(fgl,tree, N=100); +# end @@ -142,7 +142,7 @@ writeGraphPdf(fg) @async run(`evince fg.pdf`) -batchSolve(fg) +batchSolve!(fg) pl1=drawPosesLandms(fg) draw(PDF("after.pdf",20cm,20cm),pl1) From 67f12bdaacbd75aa82fb4fb4570e806b986843a0 Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 14 Sep 2018 10:35:22 -0400 Subject: [PATCH 31/47] work in progress vels in body frame --- .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 56 +++++++++++++------ examples/wheeled/racecar/racecarUtils.jl | 21 ++++++- .../wheeled/racecar/visualizationUtils.jl | 31 ++++++++-- 3 files changed, 84 insertions(+), 24 deletions(-) diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index 363eadce4..7d1809a9d 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -1,7 +1,7 @@ # Local compute version # add more julia processes -nprocs() < 5 ? addprocs(5-nprocs()) : nothing +nprocs() < 4 ? addprocs(4-nprocs()) : nothing using Caesar, RoME, Distributions using YAML, JLD, HDF5 @@ -43,12 +43,12 @@ Rz = RotZ(-pi/2) bTc= LinearMap(Rz) ∘ LinearMap(Rx) -datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" +# datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" # 175:5:370 # datafolder = ENV["HOME"]*"/data/racecar/labrun2/" # datafolder = ENV["HOME"]*"/data/racecar/labrun3/" # datafolder = ENV["HOME"]*"/data/racecar/labrun5/" -# datafolder = ENV["HOME"]*"/data/racecar/labrun6/" -# datafolder = ENV["HOME"]*"/data/racecar/labfull/" +datafolder = ENV["HOME"]*"/data/racecar/labrun6/" # 0:5:1795 +# datafolder = ENV["HOME"]*"/data/racecar/labfull/" # 0:5:1765 imgfolder = "images" @@ -63,7 +63,7 @@ mkdir(imgdir) mkdir(imgdir*"/tags") mkdir(imgdir*"/images") -camidxs = 175:5:370 #0:5:1765 +camidxs = 0:5:1795 fid = open(imgdir*"/readme.txt", "w") println(fid, datafolder) @@ -129,7 +129,7 @@ for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen # writeGraphPdf(fg) - if psid % 20 == 0 || psid == maxlen + if psid % 50 == 0 || psid == maxlen IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym)_presolve.jld") tree = wipeBuildNewTree!(fg, drawpdf=true) inferOverTree!(fg,tree, N=N) @@ -149,28 +149,61 @@ for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen prev_psid = psid end + IIF.savejld(fg, file=imgdir*"/racecar_fg_final.jld") # fg, = loadjld(file=imgdir*"/racecar_fg_final.jld") +results2csv(fg; dir=imgdir, filename="results.csv") + # save factor graph for later testing and evaluation batchSolve!(fg, drawpdf=true, N=N) IIF.savejld(fg, file=imgdir*"/racecar_fg_final_resolve.jld") # fgr, = loadjld(file=imgdir*"/racecar_fg_final_resolve.jld") +results2csv(fg; dir=imgdir, filename="results_resolve.csv") #,xmin=-3,xmax=6,ymin=-5,ymax=2); Gadfly.push_theme(:default) -pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean, to=3) +pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) +# Gadfly.set(:default_theme) Gadfly.draw(SVG(joinpath(imgdir,"images","final.svg"),15cm, 10cm),pl) # pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); # Gadfly.draw(PNG(joinpath(imgdir,"hist_final.png"),15cm, 10cm),pl) + + + + + drawPoses(fg, spscale=1.0) drawPosesLandms(fg, spscale=0.5) +getAllLandmGifs(fg, IMGS, show=false, dir=imgdir*"/") + + +ls(fg) + +poserange = 1:39 + + + + +plotPoseVelAsMax(fg, 1:39) + + + +plotPose(fg, [:x1;:x5;:x10;:x15;:x20;:x25;:x30;:x35], levels=1) + + +plotPose(fg, [:x15;:x20], levels=1) + + + + + sym = :x2 ls(fg, sym) pl = plotLocalProduct(fg, sym, dims=[1]) @@ -323,15 +356,6 @@ imageFactor(fg, :x3l2f1, IMGS[4], cfg) # # # -# fid = open("results.csv","w") -# for sym in [ls(fg)[1]...;ls(fg)[2]...] -# p = getVertKDE(fg, sym) -# val = string(KDE.getKDEMax(p)) -# println(fid, "$sym, $(val[2:(end-1)])") -# end -# close(fid) - - diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index edd9385b3..6ab299307 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -48,7 +48,7 @@ function addApriltags!(fg, pssym, posetags; bnoise=0.1, rnoise=0.1, lmtype=Point MvNormal([dx; dy; dth], - diagm([0.1;0.1;0.01].^2)) ) + diagm([0.05;0.05;0.03].^2)) ) end if rand() > DAerrors # regular single hypothesis @@ -75,8 +75,8 @@ function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odot addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.4;0.1;0.4].^2))), autoinit=autoinit) elseif odotype == VelPose2VelPose2 addNode!(fg, new_pssym, DynPose2(ut=round(Int, 200_000*(new_psid)))) - addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.4;0.4;0.2].^2)), - MvNormal(zeros(2),diagm([0.4;0.1].^2))), autoinit=autoinit) + addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.3;0.1;0.15].^2)), + MvNormal(zeros(2),diagm([0.2;0.1].^2))), autoinit=autoinit) end addApriltags!(fg, new_pssym, pose_tag_bag, lmtype=lmtype, fcttype=fcttype, DAerrors=DAerrors) @@ -126,6 +126,21 @@ end +function results2csv(fg; dir=".", filename="results.csv") + +if !isdir(dir*"/results") + mkdir(dir*"/results") +end +fid = open(dir*"/results/"*filename,"w") +for sym in [ls(fg)[1]...;ls(fg)[2]...] + p = getVertKDE(fg, sym) + val = string(KDE.getKDEMax(p)) + println(fid, "$sym, $(val[2:(end-1)])") +end +close(fid) + +end + # diff --git a/examples/wheeled/racecar/visualizationUtils.jl b/examples/wheeled/racecar/visualizationUtils.jl index 1b4536dda..640513603 100644 --- a/examples/wheeled/racecar/visualizationUtils.jl +++ b/examples/wheeled/racecar/visualizationUtils.jl @@ -188,11 +188,15 @@ function drawAllBearingImgs(fg, IMGS, cfg) end -function genGifLandm(fg, sym, IMGS; show=true, delay=50) +function genGifLandm(fg, sym, IMGS; show=true, delay=50, dir="") BIM = drawAllBearingImgs(fg, IMGS, cfg) TEMP = [] for ss in split.(string.(ls(fg,sym)),string(sym)) - push!(TEMP, BIM[Symbol(ss[1])][sym]) + if haskey(BIM, Symbol(ss[1])) + if haskey(BIM[Symbol(ss[1])], sym) + push!(TEMP, BIM[Symbol(ss[1])][sym]) + end + end end # mkdir("/tmp/animate_tags") @@ -203,21 +207,38 @@ function genGifLandm(fg, sym, IMGS; show=true, delay=50) save("/tmp/animate_tags/im$(i).png", TEMP[i]) end - run(`convert -delay $(delay) /tmp/animate_tags/im*.png $(sym).gif`) + run(`convert -delay $(delay) /tmp/animate_tags/im*.png $(dir)$(sym).gif`) !show ? nothing : @async run(`eog $(sym).gif`) return "$(sym).gif" end -function getAllLandmGifs(fg, IMGS; show=false) +function getAllLandmGifs(fg, IMGS; show=false, dir="") info("Dropping gifs here $(Base.pwd())") for l in ls(fg)[2] - genGifLandm(fg, l, IMGS, show=show) + genGifLandm(fg, l, IMGS, show=show, dir=dir) end nothing end +function plotPoseVelAsMax(fg, poserange) + + xx = Symbol.(string.("x",collect(poserange))) + + len = length(xx) + VV = zeros(len, 2) + i = 0 + for state in KDE.getKDEMax.(getVertKDE.(fg, xx)) + i += 1 + VV[i,:] = state[4:5] + end + + Gadfly.plot( + Gadfly.layer(x=poserange, y=VV[:,1], Geom.line, Theme(default_color=colorant"red")), + Gadfly.layer(x=poserange, y=VV[:,2], Geom.line, Theme(default_color=colorant"green")) + ) +end # From 4a9fac87a28afc2ff76e199211046596ff626744 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Fri, 14 Sep 2018 11:21:47 -0500 Subject: [PATCH 32/47] PPBR is issue somewhere - WIP --- examples/marine/auv/Sandshark/Sandshark_2.jl | 346 ++++++++++--------- 1 file changed, 187 insertions(+), 159 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index f3515ee5b..ee65bf355 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -12,7 +12,8 @@ using ProgressMeter const TU = TransformUtils -datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") +# datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") +datadir = joinpath(ENV["HOME"],"data","sandshark","full_wombat_2018_07_09","extracted") matcheddir = joinpath(datadir, "matchedfilter", "particles") beamdir = joinpath(datadir, "beamformer", "particles") @@ -49,14 +50,19 @@ navkeys = sort(collect(keys(navdata))) # lat,long = 9,10 # time,pitch,roll,heading,speed,[Something], internal_x,internal_y,internal_lat,internal_long, yaw_rad -# heading_to_pi(val) -# ret = val; -# ret = 2*pi - ret; -# ret = ret - 1.5*pi; -# if ret < -pi -# ret = ret + 2*pi; -# end -# end +# LBL data - note the timestamps need to be exported as float in future. +lbldata = Dict{Int, Vector{Float64}}() +lblfile = readdlm(joinpath(datadir, "lbl.csv")) +for row in lblfile + s = split(row, ",") + id = round(Int, 1000*parse(s[1])) + @show s + if s[2] != "NaN" + lbldata[id] = parse.(s) + end +end +lblkeys = sort(collect(keys(lbldata))) + # GET Y = north, X = East, Heading along +Y clockwise [0,360)] east = Float64[] @@ -81,16 +87,32 @@ for id in navkeys push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4))) ) # rotation about +Z end +lblX = Float64[] +lblY = Float64[] +for id in lblkeys + push!(lblX, getindex(lbldata[id],3) ) + push!(lblY, -getindex(lbldata[id],2) ) +end -# tssubset = navkeys[51:60] -# navdf = DataFrame( -# ts = navkeys - navkeys[1], -# x = X, -# y = Y -# ) -# pl = Gadfly.plot(navdf, x=:x, y=:y, Geom.path()) +# Plot LBL vs. NAV +tssubset = navkeys +navdf = DataFrame( + ts = navkeys - navkeys[1], + x = X, + y = Y +) +pl = [] +push!(pl, Gadfly.layer(navdf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"red"))) + +lbldf = DataFrame( + ts = lblkeys - lblkeys[1], + x = lblX, + y = lblY +) +push!(pl, Gadfly.layer(lbldf, x=:x, y=:y, Geom.path()), Theme(default_color=colorant"green")) +Gadfly.plot(pl...) # Gadfly.draw(Gadfly.PNG("/tmp/navss.png", 30cm, 20cm),pl) -# + interp_x = LinearInterpolation(navkeys, X) interp_y = LinearInterpolation(navkeys, Y) interp_yaw = LinearInterpolation(navkeys, yaw) @@ -100,7 +122,7 @@ ppbrDict = Dict{Int, Pose2Point2BearingRange}() odoDict = Dict{Int, Pose2Pose2}() # We have 261 timestamps -epochs = timestamps[50:2:261] +epochs = timestamps[11:2:200] NAV = Dict{Int, Vector{Float64}}() lastepoch = 0 for ep in epochs @@ -144,10 +166,10 @@ end function buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw)::IncrementalInference.FactorGraph fg = initfg() - # Don't care about beacon right now - # addNode!(fg, :l1, Point2) + # Add a central beacon with a prior + addNode!(fg, :l1, Point2) # Pinger location is x=16, y=0.6 - # addFactor!(fg, [curvar], IIF.Prior( MvNormal([16;0.6], diagm([0.1;0.1].^2)) )) + addFactor!(fg, [:l1], IIF.Prior( MvNormal([0.6; -16], diagm([0.1;0.1].^2)) )) index = 0 for ep in epochs @@ -161,6 +183,7 @@ function buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.05].^2)) )) end + addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) index+=1 end @@ -180,7 +203,9 @@ function buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, in for ep in epochs curvar = Symbol("x$index") addNode!(fg, curvar, Pose2) - addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + # if ep in epochs[50] + # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + # end if ep != epochs[1] addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) else @@ -188,6 +213,7 @@ function buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, in println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.05].^2)) )) end + addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) index+=1 end @@ -238,12 +264,12 @@ end # pl = drawPoses(fg, spscale=2.5) -function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y) +function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY, rosePlotStep=5) PLL = [] xx, = ls(fg) idx = 0 - for ep in epochs - idx += 1 + for ep in epochs[1:rosePlotStep:end] + idx += rosePlotStep theta = getKDEMax(getVertKDE(fg, xx[idx])) wRr = TU.R(theta[3]) pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=wRr, wTRr=theta[1:2], scale=5.0) @@ -259,60 +285,108 @@ function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y) ) # pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) push!(pllandmarks.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"red"))[1]) + lbldf = DataFrame( + ts = lblkeys - lblkeys[1], + x = lblX, + y = lblY + ) + push!(pllandmarks.layers, Gadfly.layer(lbldf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"green"))[1]) # Gadfly.plot(pl.layers) - push!(PLL, Coord.Cartesian(xmin=-150.0,xmax=10.0,ymin=-120.0,ymax=20.0)) - pla = plot([PLL;pllandmarks.layers; ]...) + # Make X/Y range same so no distorted + # push!(PLL, Coord.Cartesian(xmin=-160.0,xmax=10.0,ymin=-135.0,ymax=35.0)) + pla = plot([PLL;pllandmarks.layers; ]..., + Guide.manual_color_key("Legend", + ["Particle Filter Est.", "LBL Path", "Non-Gaussian SLAM"], ["red", "green", "light blue"])) return pla end - # Various graph options - choose one # fg = buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) fg = buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) # fg = buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) # Solvery! Roll dice for solvery check -writeGraphPdf(fg) +# writeGraphPdf(fg) # ensureAllInitialized!(fg) +t = string(now()) +savejld(fg, file="presolve_$t.jld") IIF.batchSolve!(fg) #, N=100 +savejld(fg, file="postsolve_$t.jld") +# pl = drawPoses(fg, spscale=2.75) # Just for odo plot # Roll again for inspiration check -pl = drawPoses(fg, spscale=2.75) -drawPosesLandms(fg, spscale=0.75) #Means so we don't run into MM == Union() || Dict{} in -# You rolled 20! - -# Roll agin for debug check -## Debugging strange headings -posePts = get2DPoseMeans(fg) -landPts = get2DLandmMeans(fg) -## Debugging landmark bearing range - -# ppbrDict[epoch_slice[1]] -getSample(ppbrDict[epoch_slice[1]],100) - - -addNode!(fg, :l1, Point2) -addFactor!(fg, [:x0; :l1], ppbrDict[epoch_slice[1]]) - -ls(fg, :l1) -pts = IIF.approxConv(fg, :x0l1f1, :l1) +## PLOT BEAM PATTERNS +Gadfly.push_theme(:default) +pla = drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) +Gadfly.draw(PDF("sandshark-beacon_$t.pdf", 12cm, 15cm), pla) +Gadfly.draw(PNG("sandshark-beacon_$t.png", 12cm, 15cm), pla) + +#### DEBUGGGGG PPBRDict ==================== + +LL = [0.6; -16; 0] +function expectedLocalBearing(curPose, LL) + v = [LL[1] - curPose[1]; LL[2] - curPose[2]] + world = atan2(v[2], v[1]) + loc = rad2deg(TU.wrapRad(world - curPose[3])) + while loc < 0 + loc += 360 + end + return loc +end +# println("$(interp_x[ep]), $(interp_y[ep]), world yaw = $(interp_yaw[ep] * 180 / pi)") +# XX = [interp_x(ep); interp_y[ep]; interp_yaw[ep]] +# expectedLocalBearing(XX, LL) +# println("Expected local yaw = $(expectedLocalBearing(XX, LL)). Sonar local yaw = $(rad2deg(getKDEMax(ppbr.bearing)[1]))") +# xyt = se2vee(SE2(XX[1:3]) \ SE2(LL)) +# bear = rad2deg(TU.wrapRad(atan2(xyt[2],xyt[1]) -XX[3])) + +bearExp = [] +bearAcoustics = [] +for ep in epochs + XX = [interp_x(ep); interp_y[ep]; interp_yaw[ep]] + ppbr = ppbrDict[ep] + push!(bearExp, expectedLocalBearing(XX, LL)) + push!(bearAcoustics, rad2deg(getKDEMax(ppbr.bearing)[1])) + println("local beacon heading: Expected = $(expectedLocalYaw(XX, LL)). Sonar = $(rad2deg(getKDEMax(ppbr.bearing)[1]))") +end -fct = getData(getVert(fg, :x0l1f1, nt=:fnc)) +layers = [] +push!(layers, Gadfly.layer(x=1:length(bearExp), y=bearExp, Geom.path())[1], Theme(default_color=color("red"))) +push!(layers, Gadfly.layer(x=1:length(bearAcoustics), y=bearAcoustics, Geom.path(), Theme(default_color=color("blue")))) +Gadfly.plot(layers...) +# end -fct.fnc.zDim +#################################### +###################################################### -## PLOT BEAM PATTERNS +# pl = drawPoses(fg, spscale=2.75) +# drawPosesLandms(fg, spscale=0.75) #Means so we don't run into MM == Union() || Dict{} in +# You rolled 20! -Gadfly.push_theme(:default) -pla = drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y) -Gadfly.draw(PDF("sandshark-beacon.pdf", 12cm, 15cm), pla) -Gadfly.draw(PNG("sandshark-beacon.png", 12cm, 15cm), pla) +# Roll again for debug check +## Debugging strange headings +# posePts = get2DPoseMeans(fg) +# landPts = get2DLandmMeans(fg) +## Debugging landmark bearing range +# ppbrDict[epoch_slice[1]] +# getSample(ppbrDict[epoch_slice[1]],100) +# +# +# addNode!(fg, :l1, Point2) +# addFactor!(fg, [:x0; :l1], ppbrDict[epoch_slice[1]]) +# +# ls(fg, :l1) +# pts = IIF.approxConv(fg, :x0l1f1, :l1) +# +# fct = getData(getVert(fg, :x0l1f1, nt=:fnc)) +# +# fct.fnc.zDim # dev # ep = epochs[1] @@ -323,130 +397,84 @@ Gadfly.draw(PNG("sandshark-beacon.png", 12cm, 15cm), pla) # pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=TU.R(pi/2)) # Gadfly.plot(pll...) - -diff(epochs[[end;1]]) - +# +# diff(epochs[[end;1]]) ## GADFLY EXAMPLE - - -PL = [] - -push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"red"))[1]) -push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"green"))[1]) -push!(PL, Coord.Cartesian(xmin=-1.0,xmax=11, ymin=-5.0, ymax=5.0)) -push!(PL, Guide.ylabel("test")) -push!(PL, Guide.ylabel("test")) -pl = drawPoses(fg) -push!(pl.layers, ) - - - -pl = drawPoses(fg) -pl = Gadfly.plot(pl.layers...) - - - -pl = Gadfly.plot(PL...) - -@show fieldnames(pl) -push!(pl.layers, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"magenta"))[1] ) - -pl -# vstasck, hstack - - -Gadfly.push_theme(:default) -# SVG, PDF, PNG -Gadfly.draw(PDF("/tmp/testfig.pdf", 12cm, 8cm), pl) -run(`evince /tmp/testfig.pdf`) - - -### PATH + Poses - -pl = drawPosesLandms(fg, spscale=2.5) -# Add odo -navdf = DataFrame( - ts = navkeys, - x = X, - y = Y -) -# pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) -push!(pl.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path())[1]) -Gadfly.plot(pl.layers) - - -#### DEBUGGGGG==================== - -# timestamps[51] +# plotLocalProduct(fg, :x49, dims=[1;2]) +# stuff = IncrementalInference.localProduct(fg, :x49) +# plotKDE(stuff[2], dims=[1;2], levels=10) # -# fg_demo = RoME.initfg() +# # STUFF +# fsym = :x49l1f1 +# const TU = TransformUtils +# XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) +# @show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) +# bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3])) +# b = IncrementalInference.getData(fg, fsym, nt=:fnc).fnc.usrfnc! +# b.bearing +# p = plotKDE(b.range) +# Gadfly.draw(PDF("multimodal_bearing_range.pdf", 12cm, 15cm), p) # -# addNode!(fg_demo, :x1, Pose2) -# addFactor!(fg_demo, [:x1], IIF.Prior(MvNormal(zeros(3), 0.01^2*eye(3)))) +# # SAVE THIS PLOT +# # epochs = timestamps[11:2:200] +# g = getVertKDE.(fg, [:l1, :x49]) +# m1 = KDE.marginal(g[1], [1;2]) +# m2 = KDE.marginal(g[2], [1;2]) +# norm(diff(KDE.getKDEMax.([m1; m2]))) # -# ensureAllInitialized!(fg_demo) +# kde!(rand(Normal(4.0, 0.1),100)) # -# drawPoses(fg_demo) +# ############### # +# PL = [] # +# push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"red"))[1]) +# push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"green"))[1]) +# push!(PL, Coord.Cartesian(xmin=-1.0,xmax=11, ymin=-5.0, ymax=5.0)) +# push!(PL, Guide.ylabel("test")) +# push!(PL, Guide.ylabel("test")) +# pl = drawPoses(fg) +# push!(pl.layers, ) # +# pl = drawPoses(fg) +# pl = Gadfly.plot(pl.layers...) # # -# fg = RoME.initfg() # -# addNode!(fg, :x51, Pose2) -# addFactor!(fg, [:x51], IIF.Prior(MvNormal([X[1259];Y[1259];yaw[1259]], 0.001^2*eye(3)))) +# pl = Gadfly.plot(PL...) # -# addNode!(fg, :x52, Pose2) -# wXi = TU.SE2([X[1259];Y[1259];yaw[1259]]) -# wXj = TU.SE2([X[1269];Y[1269];yaw[1269]]) -# iDXj = se2vee(wXi\wXj) -# pp = Pose2Pose2(MvNormal([iDXj...], 0.001^2*eye(3)) ) -# addFactor!(fg, [:x51;:x52], pp) +# @show fieldnames(pl) +# push!(pl.layers, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"magenta"))[1] ) # -# addNode!(fg, :x53, Pose2) -# wXi = TU.SE2([X[1269];Y[1269];yaw[1269]]) -# wXj = TU.SE2([X[1279];Y[1279];yaw[1279]]) -# iDXj = se2vee(wXi\wXj) -# pp = Pose2Pose2(MvNormal([iDXj...], 0.001^2*eye(3)) ) -# addFactor!(fg, [:x52;:x53], pp) +# pl +# # vstasck, hstack # -# addNode!(fg, :x54, Pose2) -# wXi = TU.SE2([X[1279];Y[1279];yaw[1279]]) -# wXj = TU.SE2([X[1289];Y[1289];yaw[1289]]) -# iDXj = se2vee(wXi\wXj) -# pp = Pose2Pose2(MvNormal([iDXj...], 0.001^2*eye(3)) ) -# addFactor!(fg, [:x53;:x54], pp) # -# addNode!(fg, :x55, Pose2) -# wXi = TU.SE2([X[1289];Y[1289];yaw[1289]]) -# wXj = TU.SE2([X[1299];Y[1299];yaw[1299]]) -# iDXj = se2vee(wXi\wXj) -# pp = Pose2Pose2(MvNormal([iDXj...], 0.001^2*eye(3)) ) -# addFactor!(fg, [:x54;:x55], pp) +# Gadfly.push_theme(:default) +# # SVG, PDF, PNG +# Gadfly.draw(PDF("/tmp/testfig.pdf", 12cm, 8cm), pl) +# run(`evince /tmp/testfig.pdf`) # -# ensureAllInitialized!(fg) # -# # addNode!(fg, :x56, Pose2) -# # addNode!(fg, :x57, Pose2) -# # addNode!(fg, :x58, Pose2) -# # addNode!(fg, :x59, Pose2) -# # addNode!(fg, :x60, Pose2) +# ### PATH + Poses # -# drawPoses(fg, spscale=0.1) -# -# yaw[1259]*180.0/pi -# navdata[navkeys[1259]][4] -# yaw[1259] -# -# -# 0 -# -# Gadfly.plot(x=X, x=Y, Geom.path) - - +# pl = drawPosesLandms(fg, spscale=2.5) +# # Add odo +# navdf = DataFrame( +# ts = navkeys, +# x = X, +# y = Y +# ) +# # pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) +# push!(pl.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path())[1]) +# Gadfly.plot(pl.layers) +fsym = :x49l1f1 +const TU = TransformUtils +XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) +@show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) +bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3])) # From 0dc719734c12048dc19a5ef25dee1bac76c2031d Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 14 Sep 2018 18:06:44 -0400 Subject: [PATCH 33/47] wip --- examples/wheeled/racecar/Img2AprilTagSLAM_local.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index 7d1809a9d..b07a47774 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -166,7 +166,7 @@ results2csv(fg; dir=imgdir, filename="results_resolve.csv") #,xmin=-3,xmax=6,ymin=-5,ymax=2); Gadfly.push_theme(:default) -pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) +pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:max) # Gadfly.set(:default_theme) Gadfly.draw(SVG(joinpath(imgdir,"images","final.svg"),15cm, 10cm),pl) # pl = drawPosesLandms(fg, spscale=0.1, meanmax=:mean) # ,xmin=-3,xmax=3,ymin=-2,ymax=2); @@ -181,7 +181,7 @@ drawPoses(fg, spscale=1.0) drawPosesLandms(fg, spscale=0.5) -getAllLandmGifs(fg, IMGS, show=false, dir=imgdir*"/") +getAllLandmGifs(fg, IMGS, show=false, dir=imgdir*"/images/") ls(fg) From 4ee622d99a2f5b858b2ff265542ade2096e83d4f Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 14 Sep 2018 19:38:48 -0400 Subject: [PATCH 34/47] the best name ever --- examples/marine/auv/Sandshark/devTesting.jl | 117 ++++++++++++++++++++ 1 file changed, 117 insertions(+) create mode 100644 examples/marine/auv/Sandshark/devTesting.jl diff --git a/examples/marine/auv/Sandshark/devTesting.jl b/examples/marine/auv/Sandshark/devTesting.jl new file mode 100644 index 000000000..36e1593b2 --- /dev/null +++ b/examples/marine/auv/Sandshark/devTesting.jl @@ -0,0 +1,117 @@ +using RoME, Distributions, TransformUtils +# dev test script for Pose2 and Pose2Point2BearingRange using bss sampler +using RoMEPlotting + +const TU = TransformUtils + +# drive in a circle (copy hexagonal increase) + + +# start with an empty factor graph object +N = 100 +fg = initfg() + +# Add the first pose :x0 +addNode!(fg, :x0, Pose2) + +# Add at a fixed location PriorPose2 to pin :x0 to a starting location +addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*eye(3)))) + +# Drive around in a hexagon +n_sides = 16 +side_length = 3.0 +for i in 0:(n_sides-1) + psym = Symbol("x$i") + nsym = Symbol("x$(i+1)") + addNode!(fg, nsym, Pose2) + pp = Pose2Pose2(MvNormal([side_length;0;2pi/n_sides], diagm([0.01;0.01;0.001].^2))) + addFactor!(fg, [psym;nsym], pp ) +end + +# two acoustic beacons + +addNode!(fg, :l1, Point2) +addNode!(fg, :l2, Point2) + +pL1 = IIF.Prior(MvNormal([50.0; 50.0], 0.1^2*eye(2))) +pL2 = IIF.Prior(MvNormal([-50.0; 50.0], 0.1^2*eye(2))) +addFactor!(fg,[:l1],pL1) +addFactor!(fg,[:l2],pL2) + +# doautoinit!(fg, :l1, N=N) +# doautoinit!(fg, :l2, N=N) +ensureAllInitialized!(fg) + + + +# build probabilities from parametric Gaussian but use bss sampler + +# calculate the range bearings from each pose to + +GTposes = KDE.getKDEMax.(getVertKDE.(fg, ls(fg)[1])) +GTlandm = KDE.getKDEMax.(getVertKDE.(fg, ls(fg)[2])) + +brDict = Dict{Symbol, Dict{Symbol,Any}}() + +for poseidx in 1:length(GTposes), lmidx in 1:length(GTlandm) + gtpose = GTposes[poseidx] + landm = GTlandm[lmidx] + + px, py = gtpose[1], gtpose[2] + lx, ly = landm[1], landm[2] + delta = [landm[1]-gtpose[1];landm[2]-gtpose[2]] + d = norm(delta) + wyaw = atan2(delta[2], delta[1]) + # must still incorporate pose yaw + ryaw = TU.wrapRad(wyaw - gtpose[3]) + + # generate range measurement samplable belief + r_rng = collect(linspace(0, 2*d, 128)) # range vector from 0 to two times the distance + n_rng = Normal(d, 0.1) + w_rng = pdf(n_rng, r_rng) # weights + # TODO: check for weight renormalization + bss_rng = IIF.AliasingScalarSampler(r_rng, w_rng) + + r_bear = collect(linspace(-pi, pi, 512)) # range vector from 0 to t + n_bear = Normal(ryaw, 0.1) + w_bear = pdf(n_bear, r_bear) + bss_bear = IIF.AliasingScalarSampler(r_bear, w_bear) + + posesym = ls(fg)[1][poseidx] + lmsym = ls(fg)[2][lmidx] + if !haskey(brDict, posesym) + brDict[posesym] = Dict{Symbol, Any}() + end + brDict[posesym][lmsym] = Pose2Point2BearingRange(bss_bear, bss_rng) +end + + +for posesym in [:x1], lmsym in [:l1;:l2] + addFactor!(fg, [posesym;lmsym], brDict[posesym][lmsym]) +end + +writeGraphPdf(fg,engine="dot") + +# t = string(now()) +# savejld(fg, file="presolve_$t.jld") +IIF.batchSolve!(fg) +# savejld(fg, file="postsolve_$t.jld") + + +using Gadfly +# show that it all works +# poses +pl = drawPoses(fg, spscale=1.0) + +# landmarks +pl = drawPosesLandms(fg, spscale=2.0) +Gadfly.draw(Gadfly.PDF("/tmp/test3.pdf", 20cm, 10cm),pl) + + +stuff = plotLocalProduct(fg, :x1) + + +# then use this to validate the Sandshark pipeline + +# pvt: make this an example or test somewhere? +# loosen From 2399123866467a577065877b3da4cd779214fa34 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Fri, 14 Sep 2018 19:10:02 -0500 Subject: [PATCH 35/47] Cleanup --- examples/marine/auv/Sandshark/Plotting.jl | 52 +++++ examples/marine/auv/Sandshark/Sandshark_2.jl | 229 ++++--------------- 2 files changed, 93 insertions(+), 188 deletions(-) create mode 100644 examples/marine/auv/Sandshark/Plotting.jl diff --git a/examples/marine/auv/Sandshark/Plotting.jl b/examples/marine/auv/Sandshark/Plotting.jl new file mode 100644 index 000000000..c38e14c3a --- /dev/null +++ b/examples/marine/auv/Sandshark/Plotting.jl @@ -0,0 +1,52 @@ +function layerBeamPatternRose(bear::BallTreeDensity; scale::Float64=1.0, c=colorant"magenta", wRr=TU.R(0.0),wTRr=zeros(2)) + tp = reshape([0:0.01:(2pi);], 1, :) + belp = scale*bear(tp) + Gadfly.plot(x=tp, y=belp, Geom.path) + belRose = zeros(2, length(tp)) + belRose[1,:] = belp + + idx = 0 + for rRc in TU.R.(tp) + idx += 1 + belRose[:,idx] = wRr*rRc*belRose[:,idx] + wTRr + end + + Gadfly.layer(x=belRose[1,:], y=belRose[2,:], Geom.path, Theme(default_color=c)) +end + +function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) + PLL = [] + xx, = ls(fg) + idx = 0 + for ep in epochs[1:end] + idx += 1 + theta = getKDEMax(getVertKDE(fg, xx[idx])) + wRr = TU.R(theta[3]) + pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=wRr, wTRr=theta[1:2], scale=5.0) + push!(PLL, pll) + end + + pllandmarks = drawPosesLandms(fg, spscale=2.5) + # Add odo + navdf = DataFrame( + ts = navkeys, + x = X, + y = Y + ) + # pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) + push!(pllandmarks.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"red"))[1]) + lbldf = DataFrame( + ts = lblkeys - lblkeys[1], + x = lblX, + y = lblY + ) + push!(pllandmarks.layers, Gadfly.layer(lbldf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"green"))[1]) + # Gadfly.plot(pl.layers) + + # Make X/Y range same so no distorted + # push!(PLL, Coord.Cartesian(xmin=-160.0,xmax=10.0,ymin=-135.0,ymax=35.0)) + pla = plot([PLL;pllandmarks.layers; ]..., + Guide.manual_color_key("Legend", + ["Particle Filter Est.", "LBL Path", "Non-Gaussian SLAM"], ["red", "green", "light blue"])) + return pla +end diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index ee65bf355..f4db747df 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -12,6 +12,8 @@ using ProgressMeter const TU = TransformUtils +include("Plotting.jl") + # datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") datadir = joinpath(ENV["HOME"],"data","sandshark","full_wombat_2018_07_09","extracted") matcheddir = joinpath(datadir, "matchedfilter", "particles") @@ -65,21 +67,20 @@ lblkeys = sort(collect(keys(lbldata))) # GET Y = north, X = East, Heading along +Y clockwise [0,360)] -east = Float64[] -north = Float64[] -heading = Float64[] +# east = Float64[] +# north = Float64[] +# heading = Float64[] # WANT X = North, Y = West, Yaw is right and rule from +X (0) towards +Y pi/2, [-pi,pi) # so the drawPoses picture will look flipped from Nicks picture # remember theta = atan2(y,x) # this is right hand rule - X = Float64[] Y = Float64[] yaw = Float64[] for id in navkeys - push!(east, getindex(navdata[id],7)) # x-column csv - push!(north, getindex(navdata[id],8)) # y-column csv - push!(heading, getindex(navdata[id],4)) + # push!(east, getindex(navdata[id],7)) # x-column csv + # push!(north, getindex(navdata[id],8)) # y-column csv + # push!(heading, getindex(navdata[id],4)) # push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4)))) push!(X, getindex(navdata[id],8) ) @@ -94,54 +95,30 @@ for id in lblkeys push!(lblY, -getindex(lbldata[id],2) ) end -# Plot LBL vs. NAV -tssubset = navkeys -navdf = DataFrame( - ts = navkeys - navkeys[1], - x = X, - y = Y -) -pl = [] -push!(pl, Gadfly.layer(navdf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"red"))) - -lbldf = DataFrame( - ts = lblkeys - lblkeys[1], - x = lblX, - y = lblY -) -push!(pl, Gadfly.layer(lbldf, x=:x, y=:y, Geom.path()), Theme(default_color=colorant"green")) -Gadfly.plot(pl...) -# Gadfly.draw(Gadfly.PNG("/tmp/navss.png", 30cm, 20cm),pl) - +# Build interpolators for x, y, yaw interp_x = LinearInterpolation(navkeys, X) interp_y = LinearInterpolation(navkeys, Y) interp_yaw = LinearInterpolation(navkeys, yaw) -## SELECT SEGMENT OF DATA TO WORK WITH +## Caching factors ppbrDict = Dict{Int, Pose2Point2BearingRange}() odoDict = Dict{Int, Pose2Pose2}() +NAV = Dict{Int, Vector{Float64}}() -# We have 261 timestamps +# Step: Selecting a subset for processing and build up a cache of the factors. epochs = timestamps[11:2:200] -NAV = Dict{Int, Vector{Float64}}() lastepoch = 0 for ep in epochs if lastepoch != 0 # @show interp_yaw(ep) deltaAng = interp_yaw(ep) - interp_yaw(lastepoch) - # wDXij = - # [interp_x(ep) - interp_x(lastepoch); - # interp_y(ep) - interp_y(lastepoch)] + wXi = TU.SE2([interp_x(lastepoch);interp_y(lastepoch);interp_yaw(lastepoch)]) wXj = TU.SE2([interp_x(ep);interp_y(ep);interp_yaw(ep)]) iDXj = se2vee(wXi\wXj) - # odoVec = iDXj - # odoVec = [ - # mag; - # mag; - # TransformUtils.wrapRad(deltaAng)] NAV[ep] = iDXj - println("$(iDXj[1]), $(iDXj[2]), $(iDXj[3])") + # println("$(iDXj[1]), $(iDXj[2]), $(iDXj[3])") + odoDict[ep] = Pose2Pose2(MvNormal(NAV[ep], diagm([0.1;0.1;0.005].^2))) end rangepts = rangedata[ep][:] @@ -154,159 +131,35 @@ for ep in epochs lastepoch = ep end -# Sanity check.... -for ep in epochs - x = interp_x(ep) - y = interp_y(ep) - yaw = interp_yaw(ep) - println("$ep, $x, $y, $yaw") -end - -## build the factor graph -function buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw)::IncrementalInference.FactorGraph - fg = initfg() - - # Add a central beacon with a prior - addNode!(fg, :l1, Point2) - # Pinger location is x=16, y=0.6 - addFactor!(fg, [:l1], IIF.Prior( MvNormal([0.6; -16], diagm([0.1;0.1].^2)) )) - - index = 0 - for ep in epochs - curvar = Symbol("x$index") - addNode!(fg, curvar, Pose2) - # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) - if ep != epochs[1] - addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) - else - # add a prior to the first pose location (a "GPS" prior) - println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") - addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.05].^2)) )) - end - addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) - index+=1 - end - - return fg -end - -## build the factor graph -function buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw)::IncrementalInference.FactorGraph - fg = initfg() - - # Add a central beacon with a prior - addNode!(fg, :l1, Point2) - # Pinger location is x=16, y=0.6 - addFactor!(fg, [:l1], IIF.Prior( MvNormal([0.6; -16], diagm([0.1;0.1].^2)) )) - - index = 0 - for ep in epochs - curvar = Symbol("x$index") - addNode!(fg, curvar, Pose2) - # if ep in epochs[50] - # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) - # end - if ep != epochs[1] - addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) - else - # add a prior to the first pose location (a "GPS" prior) - println("Adding a prior at $curvar, x=$(interp_x(ep)), y=$(interp_y(ep)), yaw=$(interp_yaw(ep))") - addFactor!(fg, [curvar], IIF.Prior( MvNormal([interp_x(ep);interp_y(ep);interp_yaw(ep)], diagm([0.1;0.1;0.05].^2)) )) - end - addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) - index+=1 - end - - return fg -end - -## build the factor graph -function buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw)::IncrementalInference.FactorGraph - fg = initfg() - - # Add a central beacon, no prior - addNode!(fg, :l1, Point2) - # addFactor!(fg, [:l1], IIF.Prior( MvNormal([16;0.6], diagm([0.1;0.1].^2)) )) - setVal!(fg, :l1, zeros(2,100)) - - index = 0 - for ep in epochs - curvar = Symbol("x$index") - addNode!(fg, curvar, Pose2) - addFactor!(fg, [curvar; :l1], ppbrDict[ep]) - if ep != epochs[1] - addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep], autoinit=false) - end - index+=1 - end - # add a prior to the first pose location (a "GPS" prior) - println("Adding a prior") - addFactor!(fg, [:x0], IIF.Prior( MvNormal([interp_x(epochs[1]);interp_y(epochs[1]);interp_yaw(epochs[1])], diagm([0.1;0.1;0.05].^2)) ), autoinit=false) - - return fg -end - -function layerBeamPatternRose(bear::BallTreeDensity; scale::Float64=1.0, c=colorant"magenta", wRr=TU.R(0.0),wTRr=zeros(2)) - tp = reshape([0:0.01:(2pi);], 1, :) - belp = scale*bear(tp) - Gadfly.plot(x=tp, y=belp, Geom.path) - belRose = zeros(2, length(tp)) - belRose[1,:] = belp - - idx = 0 - for rRc in TU.R.(tp) - idx += 1 - belRose[:,idx] = wRr*rRc*belRose[:,idx] + wTRr - end +## Step: Building the factor graph +fg = initfg() +# Add a central beacon with a prior +addNode!(fg, :l1, Point2) +# Pinger location is (0.6; -16) +addFactor!(fg, [:l1], IIF.Prior( MvNormal([0.6; -16], 0.1^2*eye(2)) )) - Gadfly.layer(x=belRose[1,:], y=belRose[2,:], Geom.path, Theme(default_color=c)) -end - - -# pl = drawPoses(fg, spscale=2.5) -function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY, rosePlotStep=5) - PLL = [] - xx, = ls(fg) - idx = 0 - for ep in epochs[1:rosePlotStep:end] - idx += rosePlotStep - theta = getKDEMax(getVertKDE(fg, xx[idx])) - wRr = TU.R(theta[3]) - pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=wRr, wTRr=theta[1:2], scale=5.0) - push!(PLL, pll) +index = 0 +for ep in epochs + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + + # xi -> l1 - nonparametric factor + # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + + if ep != epochs[1] + # Odo factor x(i-1) -> xi + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + else + # Prior to the first pose location (a "GPS" prior) + initLoc = [interp_x(ep);interp_y(ep);interp_yaw(ep)] + println("Adding a prior at $curvar, $initLoc") + addFactor!(fg, [curvar], IIF.Prior( MvNormal(initLoc, diagm([0.1;0.1;0.05].^2)) )) end - - pllandmarks = drawPosesLandms(fg, spscale=2.5) - # Add odo - navdf = DataFrame( - ts = navkeys, - x = X, - y = Y - ) - # pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) - push!(pllandmarks.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"red"))[1]) - lbldf = DataFrame( - ts = lblkeys - lblkeys[1], - x = lblX, - y = lblY - ) - push!(pllandmarks.layers, Gadfly.layer(lbldf, x=:x, y=:y, Geom.path(), Theme(default_color=colorant"green"))[1]) - # Gadfly.plot(pl.layers) - - # Make X/Y range same so no distorted - # push!(PLL, Coord.Cartesian(xmin=-160.0,xmax=10.0,ymin=-135.0,ymax=35.0)) - pla = plot([PLL;pllandmarks.layers; ]..., - Guide.manual_color_key("Legend", - ["Particle Filter Est.", "LBL Path", "Non-Gaussian SLAM"], ["red", "green", "light blue"])) - return pla + # Heading partial prior + addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) + index+=1 end - -# Various graph options - choose one -# fg = buildGraphOdoOnly(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) -fg = buildGraphUsingBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) -# fg = buildGraphSurveyInBeacon(epochs, ppbrDict, odoDict, interp_x, interp_y, interp_yaw) - # Solvery! Roll dice for solvery check # writeGraphPdf(fg) # ensureAllInitialized!(fg) @@ -323,7 +176,7 @@ pla = drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) Gadfly.draw(PDF("sandshark-beacon_$t.pdf", 12cm, 15cm), pla) Gadfly.draw(PNG("sandshark-beacon_$t.png", 12cm, 15cm), pla) -#### DEBUGGGGG PPBRDict ==================== +#### DEBUG PPBRDict ==================== LL = [0.6; -16; 0] function expectedLocalBearing(curPose, LL) From d801c4a2a68532a95089d28e1c71861e3a2798fb Mon Sep 17 00:00:00 2001 From: GearsAD Date: Fri, 14 Sep 2018 19:19:20 -0500 Subject: [PATCH 36/47] Cleanup --- examples/marine/auv/Sandshark/Sandshark_2.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index f4db747df..1d8d92712 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -125,9 +125,9 @@ for ep in epochs rangeprob = kde!(rangepts) azipts = azidata[ep][:,1] aziprob = kde!(azipts) + # prep the factor functions ppbrDict[ep] = Pose2Point2BearingRange(aziprob, rangeprob) - lastepoch = ep end @@ -144,7 +144,7 @@ for ep in epochs addNode!(fg, curvar, Pose2) # xi -> l1 - nonparametric factor - # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + addFactor!(fg, [curvar; :l1], ppbrDict[ep]) if ep != epochs[1] # Odo factor x(i-1) -> xi From 2211d0f7bad944c371a6f5b29c6f799443fe648e Mon Sep 17 00:00:00 2001 From: GearsAD Date: Fri, 14 Sep 2018 22:36:52 -0500 Subject: [PATCH 37/47] Funky21 reproduction scenario --- .../marine/auv/Sandshark/Funky21Problem.jl | 423 ++++++++++++++++++ 1 file changed, 423 insertions(+) create mode 100644 examples/marine/auv/Sandshark/Funky21Problem.jl diff --git a/examples/marine/auv/Sandshark/Funky21Problem.jl b/examples/marine/auv/Sandshark/Funky21Problem.jl new file mode 100644 index 000000000..280c6033a --- /dev/null +++ b/examples/marine/auv/Sandshark/Funky21Problem.jl @@ -0,0 +1,423 @@ +# new Sandshark example +# add more julia processes +# nprocs() < 7 ? addprocs(7-nprocs()) : nothing + +using Caesar, RoME, KernelDensityEstimate, IncrementalInference +using Interpolations +using Distributions + +using RoMEPlotting +using Gadfly, DataFrames +using ProgressMeter + +const TU = TransformUtils + +include(joinpath(Pkg.dir("Caesar"), "examples", "marine", "auv", "Sandshark","Plotting.jl")) + +# datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") +datadir = joinpath(ENV["HOME"],"data","sandshark","full_wombat_2018_07_09","extracted") +matcheddir = joinpath(datadir, "matchedfilter", "particles") +beamdir = joinpath(datadir, "beamformer", "particles") + +function loaddircsvs(datadir) + # https://docs.julialang.org/en/v0.6.1/stdlib/file/#Base.Filesystem.walkdir + datadict = Dict{Int, Array{Float64}}() + for (root, dirs, files) in walkdir(datadir) + # println("Files in $root") + for file in files + # println(joinpath(root, file)) # path to files + data = readdlm(joinpath(root, file),',') + datadict[parse(Int,split(file,'.')[1])/1000000] = data + end + end + return datadict +end + +rangedata = loaddircsvs(matcheddir) +azidata = loaddircsvs(beamdir) +timestamps = intersect(sort(collect(keys(rangedata))), sort(collect(keys(azidata)))) + + +# NAV data +navdata = Dict{Int, Vector{Float64}}() +navfile = readdlm(joinpath(datadir, "nav_data.csv")) +for row in navfile + s = split(row, ",") + id = round(Int, 1000*parse(s[1])) + # round(Int, 1000 * parse(s[1])) = 1531153292381 + navdata[id] = parse.(s) +end +navkeys = sort(collect(keys(navdata))) +# NAV colums are X,Y = 7,8 +# lat,long = 9,10 +# time,pitch,roll,heading,speed,[Something], internal_x,internal_y,internal_lat,internal_long, yaw_rad + +# LBL data - note the timestamps need to be exported as float in future. +lbldata = Dict{Int, Vector{Float64}}() +lblfile = readdlm(joinpath(datadir, "lbl.csv")) +for row in lblfile + s = split(row, ",") + id = round(Int, 1000*parse(s[1])) + @show s + if s[2] != "NaN" + lbldata[id] = parse.(s) + end +end +lblkeys = sort(collect(keys(lbldata))) + + +# GET Y = north, X = East, Heading along +Y clockwise [0,360)] +# east = Float64[] +# north = Float64[] +# heading = Float64[] + +# WANT X = North, Y = West, Yaw is right and rule from +X (0) towards +Y pi/2, [-pi,pi) +# so the drawPoses picture will look flipped from Nicks picture +# remember theta = atan2(y,x) # this is right hand rule +X = Float64[] +Y = Float64[] +yaw = Float64[] +for id in navkeys + # push!(east, getindex(navdata[id],7)) # x-column csv + # push!(north, getindex(navdata[id],8)) # y-column csv + # push!(heading, getindex(navdata[id],4)) + # push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4)))) + + push!(X, getindex(navdata[id],8) ) + push!(Y, -getindex(navdata[id],7) ) + push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4))) ) # rotation about +Z +end + +lblX = Float64[] +lblY = Float64[] +for id in lblkeys + push!(lblX, getindex(lbldata[id],3) ) + push!(lblY, -getindex(lbldata[id],2) ) +end + +# Build interpolators for x, y, yaw +interp_x = LinearInterpolation(navkeys, X) +interp_y = LinearInterpolation(navkeys, Y) +interp_yaw = LinearInterpolation(navkeys, yaw) + +## Caching factors +ppbrDict = Dict{Int, Pose2Point2BearingRange}() +odoDict = Dict{Int, Pose2Pose2}() +NAV = Dict{Int, Vector{Float64}}() + +# Step: Selecting a subset for processing and build up a cache of the factors. +epochs = timestamps[50:2:100] +lastepoch = 0 +for ep in epochs + if lastepoch != 0 + # @show interp_yaw(ep) + deltaAng = interp_yaw(ep) - interp_yaw(lastepoch) + + wXi = TU.SE2([interp_x(lastepoch);interp_y(lastepoch);interp_yaw(lastepoch)]) + wXj = TU.SE2([interp_x(ep);interp_y(ep);interp_yaw(ep)]) + iDXj = se2vee(wXi\wXj) + NAV[ep] = iDXj + # println("$(iDXj[1]), $(iDXj[2]), $(iDXj[3])") + + odoDict[ep] = Pose2Pose2(MvNormal(NAV[ep], diagm([0.1;0.1;0.005].^2))) + end + rangepts = rangedata[ep][:] + rangeprob = kde!(rangepts) + azipts = azidata[ep][:,1] + aziprob = kde!(azipts) + + # prep the factor functions + ppbrDict[ep] = Pose2Point2BearingRange(aziprob, rangeprob) + lastepoch = ep +end + + +## Step: Building the factor graph +fg = initfg() +# Add a central beacon with a prior +addNode!(fg, :l1, Point2) +# Pinger location is (0.6; -16) +addFactor!(fg, [:l1], IIF.Prior( MvNormal([0.6; -16], 0.1^2*eye(2)) )) + +index = 0 +for ep in epochs + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + + # xi -> l1 - nonparametric factor + # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) # #Hierdie lyk soos die nagmerrie + + if ep != epochs[1] + # Odo factor x(i-1) -> xi + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + else + # Prior to the first pose location (a "GPS" prior) + initLoc = [interp_x(ep);interp_y(ep);interp_yaw(ep)] + println("Adding a prior at $curvar, $initLoc") + addFactor!(fg, [curvar], IIF.Prior( MvNormal(initLoc, diagm([0.1;0.1;0.05].^2)) )) + end + # Heading partial prior + addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) + index+=1 +end + +# Just adding the first one... +addFactor!(fg, [:x0; :l1], ppbrDict[epochs[1]]) + +addFactor!(fg, [:x5; :l1], ppbrDict[epochs[6]]) + +addFactor!(fg, [:x10; :l1], ppbrDict[epochs[11]]) + +addFactor!(fg, [:x13; :l1], ppbrDict[epochs[14]]) + +addFactor!(fg, [:x15; :l1], ppbrDict[epochs[16]]) + +addFactor!(fg, [:x17; :l1], ppbrDict[epochs[18]]) +addFactor!(fg, [:x18; :l1], ppbrDict[epochs[19]]) +addFactor!(fg, [:x19; :l1], ppbrDict[epochs[20]]) +addFactor!(fg, [:x20; :l1], ppbrDict[epochs[21]]) +# addFactor!(fg, [:x21; :l1], ppbrDict[epochs[22]]) # breaks it! +addFactor!(fg, [:x22; :l1], ppbrDict[epochs[23]]) +addFactor!(fg, [:x23; :l1], ppbrDict[epochs[24]]) +addFactor!(fg, [:x24; :l1], ppbrDict[epochs[25]]) +addFactor!(fg, [:x25; :l1], ppbrDict[epochs[26]]) + + +plotKDE(ppbrDict[epochs[22]].bearing) +plotKDE(ppbrDict[epochs[22]].range) + +plotKDE(ppbrDict[epochs[23]].bearing) +plotKDE(ppbrDict[epochs[23]].range) + + +plotKDE([ppbrDict[epochs[21]].bearing; ppbrDict[epochs[22]].bearing; ppbrDict[epochs[23]].bearing], c=["red";"black";"green"]) +plotKDE([ppbrDict[epochs[21]].range; ppbrDict[epochs[22]].range; ppbrDict[epochs[23]].range], c=["red";"black";"green"]) + + +writeGraphPdf(fg, engine="dot") + +ensureAllInitialized!(fg) +batchSolve!(fg) + +drawPosesLandms(fg) + + +# IIF.wipeBuildNewTree!(fg, drawpdf=true) +# run(`evince bt.pdf`) +# run(`evince /tmp/bt.pdf`) + + +endDogLeg = [interp_x[epochs[end]]; interp_y[epochs[end]]] +estDogLeg = [get2DPoseMeans(fg)[1][end]; get2DPoseMeans(fg)[2][end]] +endDogLeg - estDogLeg + +drawPosesLandms(fg) + + + +ls(fg, :x25) + +#To Boldly Believe... The Good, the Bad, and the Unbeliefable +X25 = getVertKDE(fg, :x25) + +# i +pts = predictbelief(fg, :x21, [:x20x21f1; :x21l1f1]) +plotKDE([kde!(pts);X25], dims=[1;2], levels=1, c=["red";"green"]) + + +pts = predictbelief(fg, :x25, :) +plotKDE([kde!(pts);X25], dims=[1;2], levels=1, c=["red";"green"]) +plotKDE([kde!(pts);X25], dims=[3], levels=1, c=["red";"green"]) + + +# Solvery! Roll dice for solvery check +# writeGraphPdf(fg) +# ensureAllInitialized!(fg) +t = string(now()) +savejld(fg, file="presolve_$t.jld") +IIF.batchSolve!(fg) #, N=100 +savejld(fg, file="postsolve_$t.jld") + +# pl = drawPoses(fg, spscale=2.75) # Just for odo plot +# Roll again for inspiration check +## PLOT BEAM PATTERNS +Gadfly.push_theme(:default) +pla = drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) +Gadfly.draw(PDF("sandshark-beacon_$t.pdf", 12cm, 15cm), pla) +Gadfly.draw(PNG("sandshark-beacon_$t.png", 12cm, 15cm), pla) + + + + + + + + + + + + + + + + + + + + + +#### DEBUG PPBRDict ==================== + +LL = [0.6; -16; 0] +function expectedLocalBearing(curPose, LL) + v = [LL[1] - curPose[1]; LL[2] - curPose[2]] + world = atan2(v[2], v[1]) + loc = rad2deg(TU.wrapRad(world - curPose[3])) + while loc < 0 + loc += 360 + end + return loc +end +# println("$(interp_x[ep]), $(interp_y[ep]), world yaw = $(interp_yaw[ep] * 180 / pi)") +# XX = [interp_x(ep); interp_y[ep]; interp_yaw[ep]] +# expectedLocalBearing(XX, LL) +# println("Expected local yaw = $(expectedLocalBearing(XX, LL)). Sonar local yaw = $(rad2deg(getKDEMax(ppbr.bearing)[1]))") +# xyt = se2vee(SE2(XX[1:3]) \ SE2(LL)) +# bear = rad2deg(TU.wrapRad(atan2(xyt[2],xyt[1]) -XX[3])) + +bearExp = [] +bearAcoustics = [] +for ep in epochs + XX = [interp_x(ep); interp_y[ep]; interp_yaw[ep]] + ppbr = ppbrDict[ep] + push!(bearExp, expectedLocalBearing(XX, LL)) + push!(bearAcoustics, rad2deg(getKDEMax(ppbr.bearing)[1])) + println("local beacon heading: Expected = $(expectedLocalYaw(XX, LL)). Sonar = $(rad2deg(getKDEMax(ppbr.bearing)[1]))") +end + +layers = [] +push!(layers, Gadfly.layer(x=1:length(bearExp), y=bearExp, Geom.path())[1], Theme(default_color=color("red"))) +push!(layers, Gadfly.layer(x=1:length(bearAcoustics), y=bearAcoustics, Geom.path(), Theme(default_color=color("blue")))) +Gadfly.plot(layers...) +# end + +#################################### + + + +###################################################### + +# pl = drawPoses(fg, spscale=2.75) +# drawPosesLandms(fg, spscale=0.75) #Means so we don't run into MM == Union() || Dict{} in +# You rolled 20! + +# Roll again for debug check +## Debugging strange headings +# posePts = get2DPoseMeans(fg) +# landPts = get2DLandmMeans(fg) +## Debugging landmark bearing range + +# ppbrDict[epoch_slice[1]] +# getSample(ppbrDict[epoch_slice[1]],100) +# +# +# addNode!(fg, :l1, Point2) +# addFactor!(fg, [:x0; :l1], ppbrDict[epoch_slice[1]]) +# +# ls(fg, :l1) +# pts = IIF.approxConv(fg, :x0l1f1, :l1) +# +# fct = getData(getVert(fg, :x0l1f1, nt=:fnc)) +# +# fct.fnc.zDim + +# dev +# ep = epochs[1] +# # azidata[ep][:,1] +# pll = layerBeamPatternRose(ppbrDict[ep].bearing) +# Gadfly.plot(pll...) +# +# pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=TU.R(pi/2)) +# Gadfly.plot(pll...) + +# +# diff(epochs[[end;1]]) + + +## GADFLY EXAMPLE +# plotLocalProduct(fg, :x49, dims=[1;2]) +# stuff = IncrementalInference.localProduct(fg, :x49) +# plotKDE(stuff[2], dims=[1;2], levels=10) +# +# # STUFF +# fsym = :x49l1f1 +# const TU = TransformUtils +# XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) +# @show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) +# bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3])) +# b = IncrementalInference.getData(fg, fsym, nt=:fnc).fnc.usrfnc! +# b.bearing +# p = plotKDE(b.range) +# Gadfly.draw(PDF("multimodal_bearing_range.pdf", 12cm, 15cm), p) +# +# # SAVE THIS PLOT +# # epochs = timestamps[11:2:200] +# g = getVertKDE.(fg, [:l1, :x49]) +# m1 = KDE.marginal(g[1], [1;2]) +# m2 = KDE.marginal(g[2], [1;2]) +# norm(diff(KDE.getKDEMax.([m1; m2]))) +# +# kde!(rand(Normal(4.0, 0.1),100)) +# +# ############### +# +# PL = [] +# +# push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"red"))[1]) +# push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"green"))[1]) +# push!(PL, Coord.Cartesian(xmin=-1.0,xmax=11, ymin=-5.0, ymax=5.0)) +# push!(PL, Guide.ylabel("test")) +# push!(PL, Guide.ylabel("test")) +# pl = drawPoses(fg) +# push!(pl.layers, ) +# +# pl = drawPoses(fg) +# pl = Gadfly.plot(pl.layers...) +# +# +# +# pl = Gadfly.plot(PL...) +# +# @show fieldnames(pl) +# push!(pl.layers, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"magenta"))[1] ) +# +# pl +# # vstasck, hstack +# +# +# Gadfly.push_theme(:default) +# # SVG, PDF, PNG +# Gadfly.draw(PDF("/tmp/testfig.pdf", 12cm, 8cm), pl) +# run(`evince /tmp/testfig.pdf`) +# +# +# ### PATH + Poses +# +# pl = drawPosesLandms(fg, spscale=2.5) +# # Add odo +# navdf = DataFrame( +# ts = navkeys, +# x = X, +# y = Y +# ) +# # pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) +# push!(pl.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path())[1]) +# Gadfly.plot(pl.layers) + +fsym = :x49l1f1 +const TU = TransformUtils +XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) +@show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) +bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3])) + +# From 498810d581588c1057fff5c746b3398c3b73bba6 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sat, 15 Sep 2018 00:54:33 -0500 Subject: [PATCH 38/47] Doglegging --- .../marine/auv/Sandshark/Funky21Problem.jl | 105 +++++- .../marine/auv/Sandshark/FunkyContinuous.jl | 334 ++++++++++++++++++ examples/marine/auv/Sandshark/Sandshark_2.jl | 113 +++++- 3 files changed, 545 insertions(+), 7 deletions(-) create mode 100644 examples/marine/auv/Sandshark/FunkyContinuous.jl diff --git a/examples/marine/auv/Sandshark/Funky21Problem.jl b/examples/marine/auv/Sandshark/Funky21Problem.jl index 280c6033a..759b8aa02 100644 --- a/examples/marine/auv/Sandshark/Funky21Problem.jl +++ b/examples/marine/auv/Sandshark/Funky21Problem.jl @@ -157,7 +157,7 @@ for ep in epochs addFactor!(fg, [curvar], IIF.Prior( MvNormal(initLoc, diagm([0.1;0.1;0.05].^2)) )) end # Heading partial prior - addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) + # addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) index+=1 end @@ -183,8 +183,8 @@ addFactor!(fg, [:x24; :l1], ppbrDict[epochs[25]]) addFactor!(fg, [:x25; :l1], ppbrDict[epochs[26]]) -plotKDE(ppbrDict[epochs[22]].bearing) -plotKDE(ppbrDict[epochs[22]].range) +plotKDE(ppbrDict[epochs[26]].bearing) +plotKDE(ppbrDict[epochs[26]].range) plotKDE(ppbrDict[epochs[23]].bearing) plotKDE(ppbrDict[epochs[23]].range) @@ -249,12 +249,97 @@ Gadfly.draw(PNG("sandshark-beacon_$t.png", 12cm, 15cm), pla) +## resolve funky 21 +# shows all the proposals based on the clique in the tree -- similar to plotLocalProduct on factor graph +plotTreeProduct(fg, tree, :x25) +# +# using IncrementalInference +# +# +# import RoMEPlotting: spyCliqMat +# +# function spyCliqMat(cliq::Graphs.ExVertex; showmsg=true) +# mat = deepcopy(IIF.getCliqMat(cliq, showmsg=showmsg)) +# # TODO -- add improved visualization here, iter vs skip +# mat = map(Float64, mat)*2.0-1.0 +# numlcl = size(IIF.getCliqAssocMat(cliq),1) +# mat[(numlcl+1):end,:] *= 0.9 +# mat[(numlcl+1):end,:] -= 0.1 +# numfrtl1 = floor(Int,length(cliq.attributes["data"].frontalIDs)+1) +# mat[:,numfrtl1:end] *= 0.9 +# mat[:,numfrtl1:end] -= 0.1 +# @show cliq.attributes["data"].itervarIDs +# @show cliq.attributes["data"].directvarIDs +# @show cliq.attributes["data"].msgskipIDs +# @show cliq.attributes["data"].directFrtlMsgIDs +# @show cliq.attributes["data"].directPriorMsgIDs +# sp = Gadfly.spy(mat) +# push!(sp.guides, Gadfly.Guide.title("$(cliq.attributes["label"]) || $(cliq.attributes["data"].frontalIDs) :$(cliq.attributes["data"].conditIDs)")) +# push!(sp.guides, Gadfly.Guide.xlabel("fmcmcs $(cliq.attributes["data"].itervarIDs)")) +# push!(sp.guides, Gadfly.Guide.ylabel("lcl=$(numlcl) || msg=$(size(IIF.getCliqMsgMat(cliq),1))" )) +# return sp +# end +# function spyCliqMat(bt::BayesTree, lbl::Symbol; showmsg=true) +# spyCliqMat(IIF.whichCliq(bt,lbl), showmsg=showmsg) +# end +# + + + +tree = wipeBuildNewTree!(fg, drawpdf=true, show=true) + + +import IncrementalInference: getCliqMat + +sym = :x25 +# get clique sym is in +whichCliq(tree, sym).attributes["label"] +spyCliqMat(tree, sym) + +# get all variables in clique +syms = union(getCliqSymbols(tree, sym)...) +varnum = findfirst(syms, sym) +whichpot = getData(whichCliq(tree, sym)).cliqAssocMat[:,varnum] + +# get factor ids +fids = getData(whichCliq(tree, sym)).potentials[whichpot] + +# get all factors in clique +fsyms = Symbol[] +for factor in getVert.(fg, fids) + push!(fsyms, Symbol(factor.label)) +end + +# get KDEs for the factors +pp = kde!.(approxConv.(fg, fsyms, sym)) + +# plot the actual KDE +plotKDE(pp, dims=[1;2], levels=2) + +plotKDE(pp, dims=[1;2], levels=2, legend=string.(fsyms)) + + +# Colors should not appear more than once +# in at base/ +# in #plotKDE#13 at KernelDensityEstimatePlotting/src/KernelDensityEstimatePlotting.jl:299 +# in Gadfly.Guide.ManualColorKey at Gadfly/src/guide.jl:501 +ls(fg, :x25) + + +getData(fg, :x25f1, nt=:fnc).fnc.usrfnc! + + + +getData(whichCliq(tree, sym)) +0 + + @@ -420,4 +505,18 @@ XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) @show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3])) + + + + + + + + + + + + + + # diff --git a/examples/marine/auv/Sandshark/FunkyContinuous.jl b/examples/marine/auv/Sandshark/FunkyContinuous.jl new file mode 100644 index 000000000..fb55b28c5 --- /dev/null +++ b/examples/marine/auv/Sandshark/FunkyContinuous.jl @@ -0,0 +1,334 @@ +# new Sandshark example +# add more julia processes +# nprocs() < 7 ? addprocs(7-nprocs()) : nothing + +using KernelDensityEstimate, IncrementalInference +using Distributions + +const TU = TransformUtils + +## Step: Building the factor graph +fg = initfg() +# Add a central beacon with a prior +addNode!(fg, :l1, ContinuousScalar) +# Pinger location is (0.6; -16) +addFactor!(fg, [:l1], IIF.Prior( Normal(0.6, 0.1) )) + +epochs = 50:2:100 + +index=0 +for ep in epochs + curvar = Symbol("x$index") + addNode!(fg, curvar, ContinuousScalar) + + if ep != epochs[1] + # Odo factor x(i-1) -> xi + addFactor!(fg, [Symbol("x$(index-1)"); curvar], LinearConditional(Normal(1, 0.1))) + else + # Prior to the first pose location (a "GPS" prior) + addFactor!(fg, [curvar], IIF.Prior(Normal(5,5) )) + end + # Heading partial prior + # addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) + index+=1 +end + +# Just adding the first one... +addFactor!(fg, [:x0; :l1], LinearConditional(Normal(1.0, 0.1))) + +addFactor!(fg, [:x5; :l1], LinearConditional(Normal(1.0, 0.1))) + +addFactor!(fg, [:x10; :l1], LinearConditional(Normal(1.0, 0.1))) + +addFactor!(fg, [:x13; :l1], LinearConditional(Normal(1.0, 0.1))) + +addFactor!(fg, [:x15; :l1], LinearConditional(Normal(1.0, 0.1))) + +addFactor!(fg, [:x17; :l1], LinearConditional(Normal(1.0, 0.1))) +addFactor!(fg, [:x18; :l1], LinearConditional(Normal(1.0, 0.1))) +addFactor!(fg, [:x19; :l1], LinearConditional(Normal(1.0, 0.1))) +addFactor!(fg, [:x20; :l1], LinearConditional(Normal(1.0, 0.1))) +addFactor!(fg, [:x21; :l1], LinearConditional(Normal(1.0, 0.1))) # breaks it! +addFactor!(fg, [:x22; :l1], LinearConditional(Normal(1.0, 0.1))) +addFactor!(fg, [:x23; :l1], LinearConditional(Normal(1.0, 0.1))) +addFactor!(fg, [:x24; :l1], LinearConditional(Normal(1.0, 0.1))) +addFactor!(fg, [:x25; :l1], LinearConditional(Normal(1.0, 0.1))) + +plotPose(fg, [:x21]) +plotKDE(fg, [:x20, :x21, :x22, :x23, :x24]) +# plotKDE(ppbrDict[epochs[26]].range) +# +# plotKDE(ppbrDict[epochs[23]].bearing) +plotKDE(ppbrDict[epochs[23]].range) + +writeGraphPdf(fg, engine="dot") + +ensureAllInitialized!(fg) +batchSolve!(fg) + +drawPosesLandms(fg) + +savejld(fg, file="presolve_$t.jld") +IIF.batchSolve!(fg) #, N=100 +savejld(fg, file="postsolve_$t.jld") + +# pl = drawPoses(fg, spscale=2.75) # Just for odo plot +# Roll again for inspiration check +## PLOT BEAM PATTERNS +Gadfly.push_theme(:default) +pla = drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) +Gadfly.draw(PDF("sandshark-beacon_$t.pdf", 12cm, 15cm), pla) +Gadfly.draw(PNG("sandshark-beacon_$t.png", 12cm, 15cm), pla) + + + + +## resolve funky 21 + + +# shows all the proposals based on the clique in the tree -- similar to plotLocalProduct on factor graph +plotTreeProduct(fg, tree, :x25) +# +# using IncrementalInference +# +# +# import RoMEPlotting: spyCliqMat +# +# function spyCliqMat(cliq::Graphs.ExVertex; showmsg=true) +# mat = deepcopy(IIF.getCliqMat(cliq, showmsg=showmsg)) +# # TODO -- add improved visualization here, iter vs skip +# mat = map(Float64, mat)*2.0-1.0 +# numlcl = size(IIF.getCliqAssocMat(cliq),1) +# mat[(numlcl+1):end,:] *= 0.9 +# mat[(numlcl+1):end,:] -= 0.1 +# numfrtl1 = floor(Int,length(cliq.attributes["data"].frontalIDs)+1) +# mat[:,numfrtl1:end] *= 0.9 +# mat[:,numfrtl1:end] -= 0.1 +# @show cliq.attributes["data"].itervarIDs +# @show cliq.attributes["data"].directvarIDs +# @show cliq.attributes["data"].msgskipIDs +# @show cliq.attributes["data"].directFrtlMsgIDs +# @show cliq.attributes["data"].directPriorMsgIDs +# sp = Gadfly.spy(mat) +# push!(sp.guides, Gadfly.Guide.title("$(cliq.attributes["label"]) || $(cliq.attributes["data"].frontalIDs) :$(cliq.attributes["data"].conditIDs)")) +# push!(sp.guides, Gadfly.Guide.xlabel("fmcmcs $(cliq.attributes["data"].itervarIDs)")) +# push!(sp.guides, Gadfly.Guide.ylabel("lcl=$(numlcl) || msg=$(size(IIF.getCliqMsgMat(cliq),1))" )) +# return sp +# end +# function spyCliqMat(bt::BayesTree, lbl::Symbol; showmsg=true) +# spyCliqMat(IIF.whichCliq(bt,lbl), showmsg=showmsg) +# end +# + + + +tree = wipeBuildNewTree!(fg, drawpdf=true, show=true) + + +import IncrementalInference: getCliqMat + +sym = :x21 +whichCliq(tree, sym).attributes["label"] +spyCliqMat(tree, sym) + +syms = union(getCliqSymbols(tree, :x21)...) +findfirst(syms, :x21) + + + + +getData(whichCliq(tree, :x21)) + + + + + + + + + + + +0 + + + + + + + + + + + + + +#### DEBUG PPBRDict ==================== + +LL = [0.6; -16; 0] +function expectedLocalBearing(curPose, LL) + v = [LL[1] - curPose[1]; LL[2] - curPose[2]] + world = atan2(v[2], v[1]) + loc = rad2deg(TU.wrapRad(world - curPose[3])) + while loc < 0 + loc += 360 + end + return loc +end +# println("$(interp_x[ep]), $(interp_y[ep]), world yaw = $(interp_yaw[ep] * 180 / pi)") +# XX = [interp_x(ep); interp_y[ep]; interp_yaw[ep]] +# expectedLocalBearing(XX, LL) +# println("Expected local yaw = $(expectedLocalBearing(XX, LL)). Sonar local yaw = $(rad2deg(getKDEMax(ppbr.bearing)[1]))") +# xyt = se2vee(SE2(XX[1:3]) \ SE2(LL)) +# bear = rad2deg(TU.wrapRad(atan2(xyt[2],xyt[1]) -XX[3])) + +bearExp = [] +bearAcoustics = [] +for ep in epochs + XX = [interp_x(ep); interp_y[ep]; interp_yaw[ep]] + ppbr = ppbrDict[ep] + push!(bearExp, expectedLocalBearing(XX, LL)) + push!(bearAcoustics, rad2deg(getKDEMax(ppbr.bearing)[1])) + println("local beacon heading: Expected = $(expectedLocalYaw(XX, LL)). Sonar = $(rad2deg(getKDEMax(ppbr.bearing)[1]))") +end + +layers = [] +push!(layers, Gadfly.layer(x=1:length(bearExp), y=bearExp, Geom.path())[1], Theme(default_color=color("red"))) +push!(layers, Gadfly.layer(x=1:length(bearAcoustics), y=bearAcoustics, Geom.path(), Theme(default_color=color("blue")))) +Gadfly.plot(layers...) +# end + +#################################### + + + +###################################################### + +# pl = drawPoses(fg, spscale=2.75) +# drawPosesLandms(fg, spscale=0.75) #Means so we don't run into MM == Union() || Dict{} in +# You rolled 20! + +# Roll again for debug check +## Debugging strange headings +# posePts = get2DPoseMeans(fg) +# landPts = get2DLandmMeans(fg) +## Debugging landmark bearing range + +# ppbrDict[epoch_slice[1]] +# getSample(ppbrDict[epoch_slice[1]],100) +# +# +# addNode!(fg, :l1, Point2) +# addFactor!(fg, [:x0; :l1], ppbrDict[epoch_slice[1]]) +# +# ls(fg, :l1) +# pts = IIF.approxConv(fg, :x0l1f1, :l1) +# +# fct = getData(getVert(fg, :x0l1f1, nt=:fnc)) +# +# fct.fnc.zDim + +# dev +# ep = epochs[1] +# # azidata[ep][:,1] +# pll = layerBeamPatternRose(ppbrDict[ep].bearing) +# Gadfly.plot(pll...) +# +# pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=TU.R(pi/2)) +# Gadfly.plot(pll...) + +# +# diff(epochs[[end;1]]) + + +## GADFLY EXAMPLE +# plotLocalProduct(fg, :x49, dims=[1;2]) +# stuff = IncrementalInference.localProduct(fg, :x49) +# plotKDE(stuff[2], dims=[1;2], levels=10) +# +# # STUFF +# fsym = :x49l1f1 +# const TU = TransformUtils +# XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) +# @show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) +# bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3])) +# b = IncrementalInference.getData(fg, fsym, nt=:fnc).fnc.usrfnc! +# b.bearing +# p = plotKDE(b.range) +# Gadfly.draw(PDF("multimodal_bearing_range.pdf", 12cm, 15cm), p) +# +# # SAVE THIS PLOT +# # epochs = timestamps[11:2:200] +# g = getVertKDE.(fg, [:l1, :x49]) +# m1 = KDE.marginal(g[1], [1;2]) +# m2 = KDE.marginal(g[2], [1;2]) +# norm(diff(KDE.getKDEMax.([m1; m2]))) +# +# kde!(rand(Normal(4.0, 0.1),100)) +# +# ############### +# +# PL = [] +# +# push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"red"))[1]) +# push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"green"))[1]) +# push!(PL, Coord.Cartesian(xmin=-1.0,xmax=11, ymin=-5.0, ymax=5.0)) +# push!(PL, Guide.ylabel("test")) +# push!(PL, Guide.ylabel("test")) +# pl = drawPoses(fg) +# push!(pl.layers, ) +# +# pl = drawPoses(fg) +# pl = Gadfly.plot(pl.layers...) +# +# +# +# pl = Gadfly.plot(PL...) +# +# @show fieldnames(pl) +# push!(pl.layers, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"magenta"))[1] ) +# +# pl +# # vstasck, hstack +# +# +# Gadfly.push_theme(:default) +# # SVG, PDF, PNG +# Gadfly.draw(PDF("/tmp/testfig.pdf", 12cm, 8cm), pl) +# run(`evince /tmp/testfig.pdf`) +# +# +# ### PATH + Poses +# +# pl = drawPosesLandms(fg, spscale=2.5) +# # Add odo +# navdf = DataFrame( +# ts = navkeys, +# x = X, +# y = Y +# ) +# # pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path()) +# push!(pl.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path())[1]) +# Gadfly.plot(pl.layers) + +fsym = :x49l1f1 +const TU = TransformUtils +XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) +@show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) +bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3])) + + + + + + + + + + + + + + + +# diff --git a/examples/marine/auv/Sandshark/Sandshark_2.jl b/examples/marine/auv/Sandshark/Sandshark_2.jl index 1d8d92712..3d3c6ea05 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2.jl @@ -1,6 +1,6 @@ # new Sandshark example # add more julia processes -nprocs() < 7 ? addprocs(8-nprocs()) : nothing +# nprocs() < 7 ? addprocs(7-nprocs()) : nothing using Caesar, RoME, KernelDensityEstimate, IncrementalInference using Interpolations @@ -12,7 +12,7 @@ using ProgressMeter const TU = TransformUtils -include("Plotting.jl") +include(joinpath(Pkg.dir("Caesar"), "examples", "marine", "auv", "Sandshark","Plotting.jl")) # datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") datadir = joinpath(ENV["HOME"],"data","sandshark","full_wombat_2018_07_09","extracted") @@ -106,7 +106,7 @@ odoDict = Dict{Int, Pose2Pose2}() NAV = Dict{Int, Vector{Float64}}() # Step: Selecting a subset for processing and build up a cache of the factors. -epochs = timestamps[11:2:200] +epochs = timestamps[50:2:100] lastepoch = 0 for ep in epochs if lastepoch != 0 @@ -131,6 +131,7 @@ for ep in epochs lastepoch = ep end + ## Step: Building the factor graph fg = initfg() # Add a central beacon with a prior @@ -144,7 +145,7 @@ for ep in epochs addNode!(fg, curvar, Pose2) # xi -> l1 - nonparametric factor - addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + # addFactor!(fg, [curvar; :l1], ppbrDict[ep]) # #Hierdie lyk soos die nagmerrie if ep != epochs[1] # Odo factor x(i-1) -> xi @@ -160,6 +161,79 @@ for ep in epochs index+=1 end +# Just adding the first one... +addFactor!(fg, [:x0; :l1], ppbrDict[epochs[1]]) + +addFactor!(fg, [:x5; :l1], ppbrDict[epochs[6]]) + +addFactor!(fg, [:x10; :l1], ppbrDict[epochs[11]]) + +addFactor!(fg, [:x13; :l1], ppbrDict[epochs[14]]) + +addFactor!(fg, [:x15; :l1], ppbrDict[epochs[16]]) + +addFactor!(fg, [:x17; :l1], ppbrDict[epochs[18]]) +addFactor!(fg, [:x18; :l1], ppbrDict[epochs[19]]) +addFactor!(fg, [:x19; :l1], ppbrDict[epochs[20]]) +addFactor!(fg, [:x20; :l1], ppbrDict[epochs[21]]) +# addFactor!(fg, [:x21; :l1], ppbrDict[epochs[22]]) # breaks it! +addFactor!(fg, [:x22; :l1], ppbrDict[epochs[23]]) +addFactor!(fg, [:x23; :l1], ppbrDict[epochs[24]]) +addFactor!(fg, [:x24; :l1], ppbrDict[epochs[25]]) +addFactor!(fg, [:x25; :l1], ppbrDict[epochs[26]]) + + +plotKDE(ppbrDict[epochs[22]].bearing) +plotKDE(ppbrDict[epochs[22]].range) + +plotKDE(ppbrDict[epochs[23]].bearing) +plotKDE(ppbrDict[epochs[23]].range) + + +plotKDE([ppbrDict[epochs[21]].bearing; ppbrDict[epochs[22]].bearing; ppbrDict[epochs[23]].bearing], c=["red";"black";"green"]) +plotKDE([ppbrDict[epochs[21]].range; ppbrDict[epochs[22]].range; ppbrDict[epochs[23]].range], c=["red";"black";"green"]) + + +writeGraphPdf(fg, engine="dot") + +ensureAllInitialized!(fg) +batchSolve!(fg) + +drawPosesLandms(fg) + + +# IIF.wipeBuildNewTree!(fg, drawpdf=true) +# run(`evince bt.pdf`) +# run(`evince /tmp/bt.pdf`) + +# And I'll redefine anywhere +# Anywhere I RoME +# Where I lay my head is home +# Carved upon my stone +# My body lies, but still I RoME :D +endDogLeg = [interp_x[epochs[end]]; interp_y[epochs[end]]] +estDogLeg = [get2DPoseMeans(fg)[1][end]; get2DPoseMeans(fg)[2][end]] +endDogLeg - estDogLeg + +drawPosesLandms(fg) + + + +ls(fg, :x25) + +#To Boldly Believe... The Good, the Bad, and the Unbeliefable +X25 = getVertKDE(fg, :x25) + +# i +pts = predictbelief(fg, :x21, [:x20x21f1; :x21l1f1]) +plotKDE([kde!(pts);X25], dims=[1;2], levels=1, c=["red";"green"]) + + +pts = predictbelief(fg, :x25, :) +plotKDE([kde!(pts);X25], dims=[1;2], levels=1, c=["red";"green"]) +plotKDE([kde!(pts);X25], dims=[3], levels=1, c=["red";"green"]) + + # Solvery! Roll dice for solvery check # writeGraphPdf(fg) # ensureAllInitialized!(fg) @@ -175,6 +249,26 @@ Gadfly.push_theme(:default) pla = drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) Gadfly.draw(PDF("sandshark-beacon_$t.pdf", 12cm, 15cm), pla) Gadfly.draw(PNG("sandshark-beacon_$t.png", 12cm, 15cm), pla) + + + + + + + + + + + + + + + + + + + + #### DEBUG PPBRDict ==================== @@ -330,4 +424,15 @@ XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...) @show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0])) bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3])) + + + + + + + + + + + # From e1d6dbbcdd7c50b0dee4e77b510773228edfcfc7 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sat, 15 Sep 2018 18:19:57 -0500 Subject: [PATCH 39/47] Every 25th pose, data is then pretty good --- .../auv/Sandshark/Sandshark_2bcontinued.jl | 177 ++++++++++++++++++ 1 file changed, 177 insertions(+) create mode 100644 examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl diff --git a/examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl b/examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl new file mode 100644 index 000000000..ac4dcb2ed --- /dev/null +++ b/examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl @@ -0,0 +1,177 @@ +# new Sandshark example +# add more julia processes +# nprocs() < 7 ? addprocs(7-nprocs()) : nothing + +using Caesar, RoME, KernelDensityEstimate, IncrementalInference +using Interpolations +using Distributions + +using RoMEPlotting +using Gadfly, DataFrames +using ProgressMeter + +const TU = TransformUtils + +include(joinpath(Pkg.dir("Caesar"), "examples", "marine", "auv", "Sandshark","Plotting.jl")) + +# datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") +datadir = joinpath(ENV["HOME"],"data","sandshark","full_wombat_2018_07_09","extracted") +matcheddir = joinpath(datadir, "matchedfilter", "particles") +beamdir = joinpath(datadir, "beamformer", "particles") + +function loaddircsvs(datadir) + # https://docs.julialang.org/en/v0.6.1/stdlib/file/#Base.Filesystem.walkdir + datadict = Dict{Int, Array{Float64}}() + for (root, dirs, files) in walkdir(datadir) + # println("Files in $root") + for file in files + # println(joinpath(root, file)) # path to files + data = readdlm(joinpath(root, file),',') + datadict[parse(Int,split(file,'.')[1])/1000000] = data + end + end + return datadict +end + +rangedata = loaddircsvs(matcheddir) +azidata = loaddircsvs(beamdir) +timestamps = intersect(sort(collect(keys(rangedata))), sort(collect(keys(azidata)))) + + +# NAV data +navdata = Dict{Int, Vector{Float64}}() +navfile = readdlm(joinpath(datadir, "nav_data.csv")) +for row in navfile + s = split(row, ",") + id = round(Int, 1000*parse(s[1])) + # round(Int, 1000 * parse(s[1])) = 1531153292381 + navdata[id] = parse.(s) +end +navkeys = sort(collect(keys(navdata))) +# NAV colums are X,Y = 7,8 +# lat,long = 9,10 +# time,pitch,roll,heading,speed,[Something], internal_x,internal_y,internal_lat,internal_long, yaw_rad + +# LBL data - note the timestamps need to be exported as float in future. +lbldata = Dict{Int, Vector{Float64}}() +lblfile = readdlm(joinpath(datadir, "lbl.csv")) +for row in lblfile + s = split(row, ",") + id = round(Int, 1000*parse(s[1])) + @show s + if s[2] != "NaN" + lbldata[id] = parse.(s) + end +end +lblkeys = sort(collect(keys(lbldata))) + + +# GET Y = north, X = East, Heading along +Y clockwise [0,360)] +# east = Float64[] +# north = Float64[] +# heading = Float64[] + +# WANT X = North, Y = West, Yaw is right and rule from +X (0) towards +Y pi/2, [-pi,pi) +# so the drawPoses picture will look flipped from Nicks picture +# remember theta = atan2(y,x) # this is right hand rule +X = Float64[] +Y = Float64[] +yaw = Float64[] +for id in navkeys + # push!(east, getindex(navdata[id],7)) # x-column csv + # push!(north, getindex(navdata[id],8)) # y-column csv + # push!(heading, getindex(navdata[id],4)) + # push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4)))) + + push!(X, getindex(navdata[id],8) ) + push!(Y, -getindex(navdata[id],7) ) + push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4))) ) # rotation about +Z +end + +lblX = Float64[] +lblY = Float64[] +for id in lblkeys + push!(lblX, getindex(lbldata[id],3) ) + push!(lblY, -getindex(lbldata[id],2) ) +end + +# Build interpolators for x, y, yaw +interp_x = LinearInterpolation(navkeys, X) +interp_y = LinearInterpolation(navkeys, Y) +interp_yaw = LinearInterpolation(navkeys, yaw) + +## Caching factors +ppbrDict = Dict{Int, Pose2Point2BearingRange}() +odoDict = Dict{Int, Pose2Pose2}() +NAV = Dict{Int, Vector{Float64}}() +# Step: Selecting a subset for processing and build up a cache of the factors. +epochs = timestamps[50:25:1000] +lastepoch = 0 +for ep in epochs + if lastepoch != 0 + # @show interp_yaw(ep) + deltaAng = interp_yaw(ep) - interp_yaw(lastepoch) + + wXi = TU.SE2([interp_x(lastepoch);interp_y(lastepoch);interp_yaw(lastepoch)]) + wXj = TU.SE2([interp_x(ep);interp_y(ep);interp_yaw(ep)]) + iDXj = se2vee(wXi\wXj) + NAV[ep] = iDXj + # println("$(iDXj[1]), $(iDXj[2]), $(iDXj[3])") + + odoDict[ep] = Pose2Pose2(MvNormal(NAV[ep], diagm([0.1;0.1;0.005].^2))) + end + rangepts = rangedata[ep][:] + rangeprob = kde!(rangepts) + azipts = azidata[ep][:,1] + aziprob = kde!(azipts) + + # prep the factor functions + ppbrDict[ep] = Pose2Point2BearingRange(aziprob, rangeprob) + lastepoch = ep +end + + + + +## Step: Building the factor graph +fg = initfg() +l1Uncertainty = 0.1 +nonParamStep = 1 # Number of poses between nonparametric acoustic factors +# Add a central beacon with a prior +addNode!(fg, :l1, Point2) +addFactor!(fg, [:l1], IIF.Prior( MvNormal([0.6; -16], l1Uncertainty^2*eye(2)) )) +index = 0 +for ep in epochs + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + + # xi -> l1 - nonparametric factor + if (index+1) % nonParamStep == 0 + info(" - Adding $curvar->l1 factor...") + # addFactor!(fg, [curvar; (index < length(epochs)/2 ? :l1 : :l2)], ppbrDict[ep]) + addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + end + + if ep != epochs[1] + # Odo factor x(i-1) -> xi + info(" - Adding x$(index-1)->$curvar odo factor") + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + else + # Prior to the first pose location (a "GPS" prior) + initLoc = [interp_x(ep);interp_y(ep);interp_yaw(ep)] + info(" - Adding a initial location prior at $curvar, $initLoc") + addFactor!(fg, [curvar], IIF.Prior( MvNormal(initLoc, diagm([0.1;0.1;0.05].^2)) )) + end + # Heading partial prior + info(" - Adding heading prior on $curvar") + addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) + index+=1 +end + +# writeGraphPdf(fg, engine="dot") + +ensureAllInitialized!(fg) +batchSolve!(fg) + +# drawPosesLandms(fg) +drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY, "Multi-modal ISAM (bearing+range measurement every $nonParamStep poses, l1 uncertainty $l1Uncertainty)") From dd44783bdce8196c2fe1412eb31a71eb2b330efe Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 15 Sep 2018 23:23:43 -0400 Subject: [PATCH 40/47] eod on racecar --- .../wheeled/racecar/Img2AprilTagSLAM_local.jl | 53 ++++++++++++++++--- examples/wheeled/racecar/racecarUtils.jl | 2 +- 2 files changed, 48 insertions(+), 7 deletions(-) diff --git a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl index b07a47774..f550fae35 100644 --- a/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl +++ b/examples/wheeled/racecar/Img2AprilTagSLAM_local.jl @@ -46,8 +46,8 @@ bTc= LinearMap(Rz) ∘ LinearMap(Rx) # datafolder = ENV["HOME"]*"/data/racecar/straightrun3/" # 175:5:370 # datafolder = ENV["HOME"]*"/data/racecar/labrun2/" # datafolder = ENV["HOME"]*"/data/racecar/labrun3/" -# datafolder = ENV["HOME"]*"/data/racecar/labrun5/" -datafolder = ENV["HOME"]*"/data/racecar/labrun6/" # 0:5:1795 +datafolder = ENV["HOME"]*"/data/racecar/labrun5/" # 0:5:1020 +# datafolder = ENV["HOME"]*"/data/racecar/labrun6/" # 0:5:1795 # datafolder = ENV["HOME"]*"/data/racecar/labfull/" # 0:5:1765 imgfolder = "images" @@ -63,7 +63,7 @@ mkdir(imgdir) mkdir(imgdir*"/tags") mkdir(imgdir*"/images") -camidxs = 0:5:1795 +camidxs = 0:5:1020 fid = open(imgdir*"/readme.txt", "w") println(fid, datafolder) @@ -90,6 +90,12 @@ tag_bag = prepTagBag(TAGS) @save imgdir*"/tag_det_per_pose.jld" tag_bag # @load imgdir*"/tag_det_per_pose.jld" tag_bag +fid=open(imgdir*"/tags/pose_tags.csv","w") +for pose in sort(collect(keys(tag_bag))) + println(fid, "$pose, $(collect(keys(tag_bag[pose])))") +end +close(fid) + # Factor graph construction N = 100 @@ -131,12 +137,12 @@ for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen if psid % 50 == 0 || psid == maxlen IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym)_presolve.jld") - tree = wipeBuildNewTree!(fg, drawpdf=true) - inferOverTree!(fg,tree, N=N) + # tree = wipeBuildNewTree!(fg, drawpdf=true) + # inferOverTree!(fg,tree, N=N) end + IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym).jld") ## save factor graph for later testing and evaluation - IIF.savejld(fg, file=imgdir*"/racecar_fg_$(psym).jld") # ensureAllInitialized!(fg) # pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:mean) #,xmin=-3,xmax=6,ymin=-5,ymax=2); # Gadfly.draw(PNG(joinpath(imgdir,"$(psym).png"),15cm, 10cm),pl) @@ -175,6 +181,41 @@ Gadfly.draw(SVG(joinpath(imgdir,"images","final.svg"),15cm, 10cm),pl) +# artificial loops THIS IS FOR labrun 5 +ppr = Point2Point2Range(Distributions.Normal(1.0, 0.5)) +pprm = Point2Point2Range(Distributions.Normal(2.0, 1.0)) + +addFactor!(fg, [:l12; :l10], ppr) +addFactor!(fg, [:l12; :l16], ppr) +addFactor!(fg, [:l7; :l2], ppr) + +addFactor!(fg, [:l1; :l16], pprm) +addFactor!(fg, [:l1; :l10], pprm) +addFactor!(fg, [:l1; :l2], pprm) + +addFactor!(fg, [:l0; :l16], pprm) + +addFactor!(fg, [:l6; :l9], pprm) + +addFactor!(fg, [:l6; :l3], ppr) +addFactor!(fg, [:l6; :l19], ppr) +addFactor!(fg, [:l12; :l3], ppr) +addFactor!(fg, [:l12; :l19], ppr) + +ensureAllInitialized!(fg) + + +IIF.savejld(fg, file=imgdir*"/racecar_fg_presolve.jld") +results2csv(fg; dir=imgdir, filename="results_presolve_nloops.csv") + +batchSolve!(fg, drawpdf=true, N=N) + +IIF.savejld(fg, file=imgdir*"/racecar_fg_solved1_nloops.jld") +results2csv(fg; dir=imgdir, filename="results_solved_nloops.csv") +# Gadfly.push_theme(:default) +pl = drawPosesLandms(fg, spscale=0.1, drawhist=false, meanmax=:max) +Gadfly.draw(SVG(joinpath(imgdir,"images","solve_nloops1.svg"),15cm, 10cm),pl) + drawPoses(fg, spscale=1.0) diff --git a/examples/wheeled/racecar/racecarUtils.jl b/examples/wheeled/racecar/racecarUtils.jl index 6ab299307..d40c173ce 100644 --- a/examples/wheeled/racecar/racecarUtils.jl +++ b/examples/wheeled/racecar/racecarUtils.jl @@ -75,7 +75,7 @@ function addnextpose!(fg, prev_psid, new_psid, pose_tag_bag; lmtype=Point2, odot addFactor!(fg, [prev_pssym; new_pssym], Pose2Pose2(MvNormal(zeros(3),diagm([0.4;0.1;0.4].^2))), autoinit=autoinit) elseif odotype == VelPose2VelPose2 addNode!(fg, new_pssym, DynPose2(ut=round(Int, 200_000*(new_psid)))) - addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.3;0.1;0.15].^2)), + addFactor!(fg, [prev_pssym; new_pssym], VelPose2VelPose2(MvNormal(zeros(3),diagm([0.3;0.1;0.25].^2)), MvNormal(zeros(2),diagm([0.2;0.1].^2))), autoinit=autoinit) end From 22e7c95125d33cf2359d383f056caa48ea8d790b Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sat, 15 Sep 2018 22:24:50 -0500 Subject: [PATCH 41/47] 2bcontinued --- examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl b/examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl index ac4dcb2ed..d149f03a4 100644 --- a/examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl +++ b/examples/marine/auv/Sandshark/Sandshark_2bcontinued.jl @@ -101,11 +101,11 @@ interp_y = LinearInterpolation(navkeys, Y) interp_yaw = LinearInterpolation(navkeys, yaw) ## Caching factors -ppbrDict = Dict{Int, Pose2Point2BearingRange}() +ppbrDict = Dict{Int, Pose2Point2Range}() odoDict = Dict{Int, Pose2Pose2}() NAV = Dict{Int, Vector{Float64}}() # Step: Selecting a subset for processing and build up a cache of the factors. -epochs = timestamps[50:25:1000] +epochs = timestamps[50:2:100] lastepoch = 0 for ep in epochs if lastepoch != 0 @@ -122,11 +122,11 @@ for ep in epochs end rangepts = rangedata[ep][:] rangeprob = kde!(rangepts) - azipts = azidata[ep][:,1] - aziprob = kde!(azipts) + # azipts = azidata[ep][:,1] + # aziprob = kde!(azipts) # prep the factor functions - ppbrDict[ep] = Pose2Point2BearingRange(aziprob, rangeprob) + ppbrDict[ep] = Pose2Point2Range(rangeprob) lastepoch = ep end From 32b0268571689734045f2db9adcc0cb0367d9157 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sat, 15 Sep 2018 23:09:40 -0500 Subject: [PATCH 42/47] Plotting! --- examples/marine/auv/Sandshark/Plotting.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/marine/auv/Sandshark/Plotting.jl b/examples/marine/auv/Sandshark/Plotting.jl index c38e14c3a..ba5584242 100644 --- a/examples/marine/auv/Sandshark/Plotting.jl +++ b/examples/marine/auv/Sandshark/Plotting.jl @@ -14,7 +14,7 @@ function layerBeamPatternRose(bear::BallTreeDensity; scale::Float64=1.0, c=color Gadfly.layer(x=belRose[1,:], y=belRose[2,:], Geom.path, Theme(default_color=c)) end -function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) +function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY, gTitle) PLL = [] xx, = ls(fg) idx = 0 @@ -26,7 +26,7 @@ function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) push!(PLL, pll) end - pllandmarks = drawPosesLandms(fg, spscale=2.5) + pllandmarks = drawPosesLandms(fg, spscale=2.5, drawhist=false) # Add odo navdf = DataFrame( ts = navkeys, @@ -46,7 +46,7 @@ function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY) # Make X/Y range same so no distorted # push!(PLL, Coord.Cartesian(xmin=-160.0,xmax=10.0,ymin=-135.0,ymax=35.0)) pla = plot([PLL;pllandmarks.layers; ]..., - Guide.manual_color_key("Legend", - ["Particle Filter Est.", "LBL Path", "Non-Gaussian SLAM"], ["red", "green", "light blue"])) + Guide.manual_color_key("Legend", ["Particle Filter Est.", "LBL Path", "Non-Gaussian SLAM"], ["red", "green", "light blue"]), + Guide.title(gTitle)) return pla end From 4efaa23739696a149b2cddbf3cc4b55c27f8f174 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 16 Sep 2018 00:26:58 -0400 Subject: [PATCH 43/47] udpate plotting --- examples/marine/auv/Sandshark/Plotting.jl | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/examples/marine/auv/Sandshark/Plotting.jl b/examples/marine/auv/Sandshark/Plotting.jl index ba5584242..1a2e0ad21 100644 --- a/examples/marine/auv/Sandshark/Plotting.jl +++ b/examples/marine/auv/Sandshark/Plotting.jl @@ -14,7 +14,16 @@ function layerBeamPatternRose(bear::BallTreeDensity; scale::Float64=1.0, c=color Gadfly.layer(x=belRose[1,:], y=belRose[2,:], Geom.path, Theme(default_color=c)) end -function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY, gTitle) +function drawPosesLandmarksAndOdo(fg::FactorGraph, + ppbrDict::Dict, + navkeys::Array, + X::Array, + Y::Array, + lblX::Array, + lblY::Array, + gTitle::String, + drawhist=true ) + # PLL = [] xx, = ls(fg) idx = 0 @@ -26,7 +35,7 @@ function drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY, gTitl push!(PLL, pll) end - pllandmarks = drawPosesLandms(fg, spscale=2.5, drawhist=false) + pllandmarks = drawPosesLandms(fg, spscale=2.5, drawhist=drawhist) # Add odo navdf = DataFrame( ts = navkeys, From 41323d5defa03418b7b68557de0cbbbb132c339d Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 16 Sep 2018 01:14:25 -0500 Subject: [PATCH 44/47] WIP --- .../auv/Sandshark/Sandshark_2cwhathappens.jl | 181 ++++++++++++++++++ 1 file changed, 181 insertions(+) create mode 100644 examples/marine/auv/Sandshark/Sandshark_2cwhathappens.jl diff --git a/examples/marine/auv/Sandshark/Sandshark_2cwhathappens.jl b/examples/marine/auv/Sandshark/Sandshark_2cwhathappens.jl new file mode 100644 index 000000000..78212faeb --- /dev/null +++ b/examples/marine/auv/Sandshark/Sandshark_2cwhathappens.jl @@ -0,0 +1,181 @@ +# new Sandshark example +# add more julia processes +# nprocs() < 7 ? addprocs(7-nprocs()) : nothing + +using Caesar, RoME, KernelDensityEstimate, IncrementalInference +using Interpolations +using Distributions + +using RoMEPlotting +using Gadfly, DataFrames +using ProgressMeter + +const TU = TransformUtils + +include(joinpath(Pkg.dir("Caesar"), "examples", "marine", "auv", "Sandshark","Plotting.jl")) + +# datadir = joinpath(ENV["HOME"],"data","sandshark","sample_wombat_2018_09_07","processed","extracted") +datadir = joinpath(ENV["HOME"],"data","sandshark","full_wombat_2018_07_09","extracted") +matcheddir = joinpath(datadir, "matchedfilter", "particles") +beamdir = joinpath(datadir, "beamformer", "particles") + +function loaddircsvs(datadir) + # https://docs.julialang.org/en/v0.6.1/stdlib/file/#Base.Filesystem.walkdir + datadict = Dict{Int, Array{Float64}}() + for (root, dirs, files) in walkdir(datadir) + # println("Files in $root") + for file in files + # println(joinpath(root, file)) # path to files + data = readdlm(joinpath(root, file),',') + datadict[parse(Int,split(file,'.')[1])/1000000] = data + end + end + return datadict +end + +rangedata = loaddircsvs(matcheddir) +azidata = loaddircsvs(beamdir) +timestamps = intersect(sort(collect(keys(rangedata))), sort(collect(keys(azidata)))) + + +# NAV data +navdata = Dict{Int, Vector{Float64}}() +navfile = readdlm(joinpath(datadir, "nav_data.csv")) +for row in navfile + s = split(row, ",") + id = round(Int, 1000*parse(s[1])) + # round(Int, 1000 * parse(s[1])) = 1531153292381 + navdata[id] = parse.(s) +end +navkeys = sort(collect(keys(navdata))) +# NAV colums are X,Y = 7,8 +# lat,long = 9,10 +# time,pitch,roll,heading,speed,[Something], internal_x,internal_y,internal_lat,internal_long, yaw_rad + +# LBL data - note the timestamps need to be exported as float in future. +lbldata = Dict{Int, Vector{Float64}}() +lblfile = readdlm(joinpath(datadir, "lbl.csv")) +for row in lblfile + s = split(row, ",") + id = round(Int, 1000*parse(s[1])) + @show s + if s[2] != "NaN" + lbldata[id] = parse.(s) + end +end +lblkeys = sort(collect(keys(lbldata))) + + +# GET Y = north, X = East, Heading along +Y clockwise [0,360)] +# east = Float64[] +# north = Float64[] +# heading = Float64[] + +# WANT X = North, Y = West, Yaw is right and rule from +X (0) towards +Y pi/2, [-pi,pi) +# so the drawPoses picture will look flipped from Nicks picture +# remember theta = atan2(y,x) # this is right hand rule +X = Float64[] +Y = Float64[] +yaw = Float64[] +for id in navkeys + # push!(east, getindex(navdata[id],7)) # x-column csv + # push!(north, getindex(navdata[id],8)) # y-column csv + # push!(heading, getindex(navdata[id],4)) + # push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4)))) + + push!(X, getindex(navdata[id],8) ) + push!(Y, -getindex(navdata[id],7) ) + push!(yaw, TU.wrapRad(-deg2rad(getindex(navdata[id],4))) ) # rotation about +Z +end + +lblX = Float64[] +lblY = Float64[] +for id in lblkeys + push!(lblX, getindex(lbldata[id],3) ) + push!(lblY, -getindex(lbldata[id],2) ) +end + +# Build interpolators for x, y, yaw +interp_x = LinearInterpolation(navkeys, X) +interp_y = LinearInterpolation(navkeys, Y) +interp_yaw = LinearInterpolation(navkeys, yaw) + +## Caching factors +ppbrDict = Dict{Int, Pose2Point2BearingRange}() +odoDict = Dict{Int, Pose2Pose2}() +NAV = Dict{Int, Vector{Float64}}() +# Step: Selecting a subset for processing and build up a cache of the factors. +epochs = timestamps[25:2:300] +lastepoch = 0 +for ep in epochs + if lastepoch != 0 + # @show interp_yaw(ep) + deltaAng = interp_yaw(ep) - interp_yaw(lastepoch) + + wXi = TU.SE2([interp_x(lastepoch);interp_y(lastepoch);interp_yaw(lastepoch)]) + wXj = TU.SE2([interp_x(ep);interp_y(ep);interp_yaw(ep)]) + iDXj = se2vee(wXi\wXj) + NAV[ep] = iDXj + # println("$(iDXj[1]), $(iDXj[2]), $(iDXj[3])") + + odoDict[ep] = Pose2Pose2(MvNormal(NAV[ep], diagm([0.1;0.1;0.005].^2))) + end + rangepts = rangedata[ep][:] + rangeprob = kde!(rangepts / 0.66) + azipts = azidata[ep][:,1] + aziprob = kde!(azipts) + + # prep the factor functions + ppbrDict[ep] = Pose2Point2BearingRange(aziprob, rangeprob) + lastepoch = ep +end + + + + +## Step: Building the factor graph +fg = initfg() +l1Uncertainty = 0.1 +# Add a central beacon with a prior +addNode!(fg, :l1, Point2) +addFactor!(fg, [:l1], IIF.Prior( MvNormal([0.6; -16], l1Uncertainty^2*eye(2)) )) +index = 0 +for ep in epochs + curvar = Symbol("x$index") + addNode!(fg, curvar, Pose2) + + # xi -> l1 - nonparametric factor + info(" - Adding $curvar->l1 factor...") + addFactor!(fg, [curvar; :l1], ppbrDict[ep]) + + if ep != epochs[1] + # Odo factor x(i-1) -> xi + info(" - Adding x$(index-1)->$curvar odo factor") + addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep]) + else + # Prior to the first pose location (a "GPS" prior) + initLoc = [interp_x(ep);interp_y(ep);interp_yaw(ep)] + info(" - Adding a initial location prior at $curvar, $initLoc") + addFactor!(fg, [curvar], IIF.Prior( MvNormal(initLoc, diagm([0.1;0.1;0.05].^2)) )) + end + # Heading partial prior + info(" - Adding heading prior on $curvar") + addFactor!(fg, [curvar], RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))) + index+=1 +end + + +# HANDCRANK +ppbr = ppbrDict[epochs[end]] +info("Est = $(getKDEMax(ppbr.range))") +xp = interp_x[epochs[end]]-0.6 +yp = interp_y[epochs[end]]+16 +info("Exp = $(sqrt(xp^2 + yp^2))") + +# writeGraphPdf(fg, engine="dot") + +ensureAllInitialized!(fg) +batchSolve!(fg) + +# drawPosesLandms(fg) +drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY, "Multi-modal ISAM (bearing+range measurement every $nonParamStep poses, l1 uncertainty $l1Uncertainty)") From f0b19b483ff6929667d11ec1a4a614faae8cdd15 Mon Sep 17 00:00:00 2001 From: Kevin Date: Tue, 18 Sep 2018 12:27:55 -0400 Subject: [PATCH 45/47] Added sim2d and dependeny ParseUtils.jl, ymmv --- examples/simulation/ParseUtils.jl | 30 +++++ examples/simulation/Sim2D.jl | 210 ++++++++++++++++++++++++++++++ 2 files changed, 240 insertions(+) create mode 100644 examples/simulation/ParseUtils.jl create mode 100644 examples/simulation/Sim2D.jl diff --git a/examples/simulation/ParseUtils.jl b/examples/simulation/ParseUtils.jl new file mode 100644 index 000000000..05bbccfdb --- /dev/null +++ b/examples/simulation/ParseUtils.jl @@ -0,0 +1,30 @@ +# ParseUtils.jl +# +# For now, a set of wrappers for Julia readdlm and writedlm +# +# Kevin Doherty 2018 + +# param pose_file: path to ground truth poses (ordered) +# formatted as (x, y, theta) +# param lm_file: path to ground truth landmarks (ordered) +# formatted as (x, y, class) +# expected return: pose array and landmark array +function read_poses_landmarks(pose_fname, lm_fname)::Tuple{Array{Float64}, Array{Float64}} + poses = readdlm(pose_fname, ',') + landmarks = readdlm(lm_fname, ',') + return (poses, landmarks) +end + +function write_poses_landmarks(poses, landmarks, pose_fname, lm_fname) + writedlm(pose_fname, poses, ",") + writedlm(lm_fname, landmarks, ",") +end + +function read_measurements(fname)::Array{Float64} + meas = readdlm(fname, ',') + return meas +end + +function write_measurements(meas, fname) + writedlm(fname, meas, ",") +end diff --git a/examples/simulation/Sim2D.jl b/examples/simulation/Sim2D.jl new file mode 100644 index 000000000..1e43a935d --- /dev/null +++ b/examples/simulation/Sim2D.jl @@ -0,0 +1,210 @@ +using Distributions + +include("ParseUtils.jl") + +function bearing(x1::Array{Float64,1}, x2::Array{Float64,1})::Float64 + return atan2(x2[2] - x1[2], x2[1] - x1[1]) +end + +function relative_bearing(pose::Array{Float64, 1}, point::Array{Float64, 1})::Float64 + pose2pointbearing = atan2(point[2] - pose[2], point[1] - pose[1]) + pose_bearing = pose[3] + b = pose2pointbearing - pose_bearing + if b < -pi + b += 2.0*pi + end + if b >= pi + b -= 2.0*pi + end + return b +end + +function range_meas(x1::Array{Float64,1}, x2::Array{Float64,1})::Float64 + return norm(x2 - x1) +end + +function rb_to_xy(meas::Array{Float64}, curr_pose::Array{Float64})::Array{Float64} + x, y, w = curr_pose + r, b = meas + + theta = w + b + lm_x = x + r * cos(theta) + lm_y = y + r * sin(theta) + return [lm_x; lm_y] +end + +function landmarks_in_range(pose::Array{Float64,1}, landmarks::Array{Float64}, sensor_max_range::Float64)::Array{Float64} + lm_in_range = Array{Float64,2}(size(landmarks,1),0) + for i in 1:size(landmarks,2) + lm = landmarks[:,i] + if range_meas(pose[1:2], lm[1:2]) < sensor_max_range + lm_in_range = [lm_in_range lm] + end + end + return lm_in_range +end + +# Return all landmarks in field of view +# params: +# pose::Array{Float64,1} estimated robot pose +# landmarks::Array{Float64} array of landmarks to test +# sensor_max_range::Float64 maximum sensor range (m) +# sensor_fov::Float64 total sensor field of view (rad) +# return: +# Array{Float64} array of landmarks within the field of view of the robot from the estimated pose +function landmarks_in_fov(pose::Array{Float64,1}, landmarks::Array{Float64}, sensor_max_range::Float64, sensor_fov::Float64)::Array{Float64} + lm_in_fov = Array{Float64,2}(size(landmarks,1),0) + for i in 1:size(landmarks,2) + lm = landmarks[:,i] + if range_meas(pose[1:2], lm[1:2]) < sensor_max_range && abs(relative_bearing(pose[1:3], lm[1:2])) < sensor_fov/2.0 + lm_in_fov = [lm_in_fov lm] + end + end + return lm_in_fov +end + +function measure(pose::Array{Float64,1}, + landmarks::Array{Float64}, + sensor_max_range::Float64, + bearing_noise::Float64, + range_noise::Float64)::Array{Float64} + + meas = Array{Float64,2}(2,0) + lm_in_range = landmarks_in_range(pose, landmarks, sensor_max_range) + if isempty(lm_in_range) + return [] + end + for i in 1:size(lm_in_range, 2) + lm = lm_in_range[:,i] + z = range_meas(pose[1:2], lm) + rand(Normal(0, range_noise)) + b = bearing(pose[1:2], lm) - pose[3] + rand(Normal(0, bearing_noise)) + + # Correction for bearing? + if b < 0.0 + b = b + 2*pi + end + if b > 2*pi + b = b - 2*pi + end + + meas = [meas [z; b]] + end + return meas +end + +# transform coordinate p2 (originally relative to p1) +# into global coordinate frame [0.0; 0.0; 0.0] +function transform_relative_to_global(p1, p2) + x1, y1, w1 = p1 + total_w = p2[3] + p1[3] + R = [cos(w1) -sin(w1); sin(w1) cos(w1)] + A = [R [x1; y1]; 0.0 0.0 1.0] + p2 = [p2[1:2]; 1.0] + global_p2 = A*p2 + global_p2[3] = total_w + return global_p2 +end + +# transform coordinate p2 (originally in global frame) +# into relative coordinate frame p1 +function transform_global_to_relative(p1, p2) + x1, y1, w1 = p1 + dw = p2[3] - p1[3] + R = [cos(w1) -sin(w1); sin(w1) cos(w1)] + A = [R [x1; y1]; 0.0 0.0 1.0] + p2 = [p2[1:2]; 1.0] + rel_p2 = inv(A)*p2 + rel_p2[3] = dw + return rel_p2 +end + +function measure_semantic(pose::Array{Float64,1}, + landmarks::Array{Float64}, + sensor_max_range::Float64, + sensor_fov::Float64, + bearing_noise::Float64, + range_noise::Float64, + n_classes::Int64)::Array{Float64} + + meas = Array{Float64,2}(2 + n_classes,0) + @show lm_in_fov = landmarks_in_fov(pose, landmarks, sensor_max_range, sensor_fov) + @show size(lm_in_fov) + if isempty(lm_in_fov) + return [] + end + for i in 1:size(lm_in_fov,2) + lm = lm_in_fov[:,i] + z = range_meas(pose[1:2], lm[1:2]) + rand(Normal(0, range_noise)) + b = bearing(pose[1:2], lm[1:2]) - pose[3] + rand(Normal(0, bearing_noise)) + + # Here the confusion matrix is set up for a classifier with 90% accuracy, independent of class + # This can be made a parameter in future + p = ones(n_classes) * (0.1 / (n_classes - 1)) + p[convert(Int64, lm[3])] = 0.9 + pred = rand(Categorical(p)) # Generate a random sample from confusion matrix probabilities + s = ones(n_classes) * (0.1 / (n_classes - 1)) + s[pred] = 0.9 + + # Correction for bearing + if b < -pi + b = b + 2*pi + end + + if b >= pi + b = b - 2*pi + end + + meas = [meas [z; b; s]] + end + return meas +end + +function run_sim(poses, landmarks, sensor_max_range, sensor_fov, range_noise, bearing_noise, odom_noise, n_classes; save_data=false) + sbr_data = Array{Float64, 2}(1 + 2 + n_classes, 0) + odom_data = Array{Float64, 2}(5, 0) + for i in 1:size(poses,2) + pose = poses[:,i] + if i > 1 + previous_pose = poses[:,i-1] + rel_xyz = transform_global_to_relative(previous_pose, pose + rand(MvNormal(zeros(3), odom_noise))) + odom_meas = rel_xyz + odom_meas = [i - 1; i; odom_meas] + odom_data = [odom_data odom_meas] + end + sbr_meas = measure_semantic(pose, landmarks, sensor_max_range, sensor_fov, range_noise, bearing_noise, n_classes) + if !isempty(sbr_meas) + sbr_meas = [i; sbr_meas] + sbr_data = [sbr_data sbr_meas] + end + + end + + # Write results to files + # Could param out a data directory + if save_data + stamp = Dates.format(Dates.now(), "yyyy-mm-dd-HH-MM-SS") + @show stamp + write_poses_landmarks(poses, landmarks, string("../data/poses_", stamp, ".txt"), string("../data/landmarks_", stamp, ".txt")) + write_measurements(odom_data, string("../data/odom_", stamp, ".txt")) + write_measurements(sbr_data, string("../data/sbr_", stamp, ".txt")) + end +end + +# Measurement parameters +sensor_max_range = 3.5 +sensor_fov = 2.0*pi/3.0 +landmark_sample_noise = 0.1 +range_model_noise = 0.1 +bearing_model_noise = 0.025 +odom_noise = 0.001*eye(3) +n_classes = 2 + +# Set up landmarks; would be cool to randomly generate these +landmarks = [[1.0; 1.0; 1] [1.0; -1.0; 2] [3.0; 1.0; 2] [4.0; -1.0; 2] [6.0; 1.0; 1] [6.0; -1.0; 1] [8.0; -1.0; 2] [9.0; 1.0; 1] [11.0; -1.0; 2] [11.0; 2.0; 1] [11.0; 4.0; 2] [11.0; 5.0; 1] [11.0; 7.0; 2] [11.0; 9.0; 1] [11.0; 11.0; 2] [9.0; 3.0; 2] [9.0; 4.0; 1] [9.0; 6.0; 1] [9.0; 9.0; 2] [8.0; 9.0; 1] [8.0; 11.0; 2] [7.0; 11.0; 1] [5.0; 9.0; 2] [5.0; 11.0; 2] [4.0; 11.0; 1] [3.0; 9.0; 1] [1.0; 6.0; 1] [-1.0; 10.0; 2] [-1.0; 5.0; 1]] + +# True robot poses; would be cool to randomly generate these +poses = [[0.0; 0.0; 0.0] [2.0; 0.0; 0.0] [4.0; 0.0; 0.0] [6.0; 0.0; 0.0] [8.0; 0.0; 0.0] [10.0; 0.0; pi/2] [10.0; 2.0; pi/2] [10.0; 4.0; pi/2] [10.0; 6.0; pi/2] [10.0; 8.0; pi/2] [10.0; 10.0; pi] [8.0; 10.0; pi] [6.0; 10.0; pi] [4.0; 10.0; pi] [2.0; 10.0; pi] [0.0; 10.0; 3*pi/2] [0.0; 8.0; 3*pi/2] [0.0; 6.0; 3*pi/2] [0.0; 4.0; 3*pi/2] [0.0; 2.0; 3*pi/2] [0.0; 0.0; 0.0]] + +# Run the simulation and save data files +# run_sim(poses, landmarks, sensor_max_range, sensor_fov, range_model_noise, bearing_model_noise, odom_noise, n_classes, save_data=true) + From 1a5dea22cf89fbcbe9ed5081b91ff47b8ebd8981 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 23 Sep 2018 23:42:29 -0500 Subject: [PATCH 46/47] Update CloudGraphIntegration.jl --- src/cloudgraphs/CloudGraphIntegration.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cloudgraphs/CloudGraphIntegration.jl b/src/cloudgraphs/CloudGraphIntegration.jl index 9e047353d..1c1e32332 100644 --- a/src/cloudgraphs/CloudGraphIntegration.jl +++ b/src/cloudgraphs/CloudGraphIntegration.jl @@ -643,7 +643,7 @@ function subLocalGraphCopy!( # warn("subGraphCopy! is a work in progress") conn = fgl.cg.neo4j.connection - IDs = getLblExVertexNeoIDs(conn, string.(lbls), session=fgl.sessionname, robot=fgl.robotname, user=fg.username, reqbackendset=reqbackendset, reqready=reqready, neighbors=neighbors ) + IDs = getLblExVertexNeoIDs(conn, string.(lbls), session=fgl.sessionname, robot=fgl.robotname, user=fgl.username, reqbackendset=reqbackendset, reqready=reqready, neighbors=neighbors ) println("fullSubGraphCopy: $(length(IDs)) nodes in session $(fgl.sessionname) if reqbackendset=$reqbackendset and reqready=$reqready...") copyGraphNodesEdges!(fgl, IDs) nothing From 7759ccf5d4f0c6b92b841fee027f0be267549a38 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 2 Oct 2018 15:26:08 -0400 Subject: [PATCH 47/47] Update Graphs.jl URL to JuliaAttic --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index de966a7e6..ada28a403 100644 --- a/README.md +++ b/README.md @@ -103,11 +103,11 @@ Consider citing our work: [dvis-build-url]: https://travis-ci.org/rdeits/DrakeVisualizer.jl [dvis-url]: http://www.github.com/rdeits/DrakeVisualizer.jl --> -[graphs-cov-img]: https://codecov.io/github/JuliaArchive/Graphs.jl/coverage.svg?branch=master -[graphs-cov-url]: https://codecov.io/github/JuliaArchive/Graphs.jl?branch=master -[graphs-build-img]: https://travis-ci.org/JuliaArchive/Graphs.jl.svg?branch=master -[graphs-build-url]: https://travis-ci.org/JuliaArchive/Graphs.jl -[graphs-url]: http://www.github.com/JuliaArchive/Graphs.jl +[graphs-cov-img]: https://codecov.io/github/JuliaAttic/Graphs.jl/coverage.svg?branch=master +[graphs-cov-url]: https://codecov.io/github/JuliaAttic/Graphs.jl?branch=master +[graphs-build-img]: https://travis-ci.org/JuliaAttic/Graphs.jl.svg?branch=master +[graphs-build-url]: https://travis-ci.org/JuliaAttic/Graphs.jl +[graphs-url]: http://www.github.com/JuliaAttic/Graphs.jl [cloudgraphs-cov-img]: https://codecov.io/github/GearsAD/CloudGraphs.jl/coverage.svg?branch=master [cloudgraphs-cov-url]: https://codecov.io/github/GearsAD/CloudGraphs.jl?branch=master