From ed19c2bfec5eb9919df512b686f4428c83178fcd Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 10 Apr 2018 20:03:29 -0400 Subject: [PATCH 01/27] work in progress --- examples/lcmserver/synchrony.jl | 276 ++++++++++++++++++++++++++++++++ 1 file changed, 276 insertions(+) create mode 100644 examples/lcmserver/synchrony.jl diff --git a/examples/lcmserver/synchrony.jl b/examples/lcmserver/synchrony.jl new file mode 100644 index 000000000..8ea3f2ad1 --- /dev/null +++ b/examples/lcmserver/synchrony.jl @@ -0,0 +1,276 @@ +#= +LCM Server: an LCM interface to Caesar.jl + +=# + +using Caesar, SynchronySDK +using CaesarLCMTypes +using TransformUtils, Rotations, CoordinateTransformations +using Distributions +using LCMCore +using LibBSON +# using CloudGraphs # for sorryPedro + + + +# ... Change all functions to use Synchrony instead + +function initialize!(backend_config, + user_config) + println("[Caesar.jl] Setting up factor graph") + fg = Caesar.initfg(sessionname=user_config["session"], cloudgraph=backend_config) + println("[Caesar.jl] Creating SLAM client/object") + return SLAMWrapper(fg, nothing, 0) +end + +function setRobotParameters!(cg::CloudGraph, session::AbstractString) + hauvconfig = Dict() + hauvconfig["robot"] = "hauv" + hauvconfig["bTc"] = [0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0] + hauvconfig["bTc_format"] = "xyzqwqxqyqz" + # currently unused, but upcoming + hauvconfig["pointcloud_description_name"] = "BSONpointcloud" + hauvconfig["pointcloud_color_description_name"] = "BSONcolors" + # robotdata = json(hauvconfig).data + + # Actually modify the databases + insertrobotdatafirstpose!(cg, session, hauvconfig) + nothing +end + +""" +Adds pose nodes to graph with a prior on Z, pitch, and roll. +""" +function handle_poses!(slam::SLAMWrapper, + message_data) + message = caesar.pose_node_t[:decode](message_data) + id = message[:id] + println("[Caesar.jl] Received pose message for x$(id)") + + mean = message[:mean] + covar = message[:covar] + t = [mean[1], mean[2], mean[3]] + qw = mean[4] + qxyz = [mean[5], mean[6], mean[7]] + q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? + pose = SE3(t,q) + euler = Euler(q) + + node_label = Symbol("x$(id)") + xn = addNode!(slam.fg, node_label, labels=["POSE"], dims=6) # this is an incremental inference call + slam.lastposesym = node_label; # update object + + if id == 1 + println("[Caesar.jl] First pose") + # this is the first message, and it does not carry odometry, but the prior on the first node. + + # add 6dof prior + initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) + addFactor!(slam.fg, [xn], initPosePrior) + + # auto init is coming, this code will likely be removed + initializeNode!(slam.fg, node_label) + + # set robot parameters in the first pose, this will become a separate node in the future + println("[Caesar.jl] Setting robot parameters") + setRobotParameters!(slam.fg.cg, slam.fg.sessionname) + end +end + +# handles ZPR priors on poses +function handle_priors!(slam::SLAMWrapper, + message_data) + + + message = caesar.prior_zpr_t[:decode](message_data) + id = message[:id] + println("[Caesar.jl] Adding prior on RPZ to x$(id)") + + node_label = Symbol("x$(id)") + xn = getVert(slam.fg,node_label) + + z = message[:z] + pitch = message[:pitch] + roll = message[:roll] + + var_z = message[:var_z] + var_pitch = message[:var_pitch] + var_roll = message[:var_roll] + + rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) + z_dist = Normal(z, var_z) + prior_rpz = PartialPriorRollPitchZ(rp_dist, z_dist) + addFactor!(slam.fg, [xn], prior_rpz) +end + + +function handle_partials!(slam::SLAMWrapper, + message_data) + message = caesar.pose_pose_xyh_t[:decode](message_data) + + origin_id = message[:node_1_id] + destination_id = message[:node_2_id] + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + println("[Caesar.jl] Adding XYH odometry constraint between(x$(origin_id), x$(destination_id))") + + delta_x = message[:delta_x] + delta_y = message[:delta_y] + delta_yaw = message[:delta_yaw] + + var_x = message[:var_x] + var_y = message[:var_y] + var_yaw = message[:var_yaw] + + origin_label, destination_label + xo = getVert(slam.fg,origin_label) + xd = getVert(slam.fg,destination_label) + + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = PartialPose3XYYaw(xyh_dist) + addFactor!(slam.fg, [xo;xd], xyh_factor) + + initializeNode!(slam.fg, destination_label) + println() +end + + +function handle_loops!(slaml::SLAMWrapper, + message_data) + message = caesar.pose_pose_xyh_nh_t[:decode](message_data) + + origin_id = message[:node_1_id] + destination_id = message[:node_2_id] + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + delta_x = message[:delta_x] + delta_y = message[:delta_y] + delta_yaw = message[:delta_yaw] + + var_x = message[:var_x] + var_y = message[:var_y] + var_yaw = message[:var_yaw] + confidence = message[:confidence] + + xo = getVert(slaml.fg,origin_label) + xd = getVert(slaml.fg,destination_label) + + # if (destination_id - origin_id == 1) + # warn("Avoiding parallel factor! See: https://github.com/dehann/IncrementalInference.jl/issues/63To ") + # return + # end + + println("[Caesar.jl] Adding XYH-NH loop closure constraint between (x$(origin_id), x$(destination_id))") + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = PartialPose3XYYawNH(xyh_dist ,[1.0-confidence, confidence]) # change to NH + addFactor!(slaml.fg, [xo;xd], xyh_factor ) + + # println("[Caesar.jl] Adding P3P3NH loop closure constraint between (x$(origin_id), x$(destination_id))") + + # # line below fails! + # lcf = Pose3Pose3NH( MvNormal(veeEuler(rel_pose), diagm(1.0./covar)), [0.5;0.5]) # define 50/50% hypothesis + # lcf_label = Symbol[origin_label;destination_label] + + # addFactor!(slaml.fg, lcf_label, lcf) +end + + +# type Cloud +# points:: +# colors:: +# end + +# function add_pointcloud!(slaml::SLAMWrapper, nodeID::Symbol, cloud::Cloud ) +# # fetch from database +# vert = getVert(slaml.fg, nodeID, api=IncrementalInference.dlapi) + +# # add points blob +# serialized_point_cloud = BSONObject(Dict("pointcloud" => cloud.points)) +# appendvertbigdata!(slaml.fg, vert, "BSONpointcloud", string(serialized_point_cloud).data) + +# # add colors blob +# serialized_colors = BSONObject(Dict("colors" => cloud.colors)) +# appendvertbigdata!(slaml.fg, vert, "BSONcolors", string(serialized_colors).data) +# end + + + +""" + handle_clouds(slam::SLAMWrapper, message_data) + + Callback for caesar_point_cloud_t messages. Adds point cloud to SLAM_Client +""" +function handle_clouds!(slam::SLAMWrapper, + message_data) + # TODO: interface here should be as simple as slam_client.add_pointcloud(nodeID, pc::SomeCloudType) + + # TODO: check for empty clouds! + + message = caesar.point_cloud_t[:decode](message_data) + + id = message[:id] + + last_pose = Symbol("x$(id)") + println("[Caesar.jl] Got cloud $id") + + # 2d arrays of points and colors (from LCM data into arrays{arrays}) + points = [[pt[1], pt[2], pt[3]] for pt in message[:points]] + colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in message[:colors]] + + + # TODO: check if vert exists or not (may happen if messages are lost or out of order) + vert = getVert(slam.fg, last_pose, api=IncrementalInference.dlapi) # fetch from database + + # push to mongo (using BSON as a quick fix) + # (for deserialization, see src/DirectorVisService.jl:cachepointclouds!) + serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) + appendvertbigdata!(slam.fg, vert, "BSONpointcloud", string(serialized_point_cloud).data) + serialized_colors = BSONObject(Dict("colors" => colors)) + appendvertbigdata!(slam.fg, vert, "BSONcolors", string(serialized_colors).data) +end + +# this function handles lcm messages +function listener!(slam::SLAMWrapper, + lcm_node::LCMCore.LCM) + # handle traffic + # TODO: handle termination + while true + handle(lcm_node) + end +end + + +#println("[Caesar.jl] Prompting user for configuration") +#@load "usercfg.jld" +#user_config["session"] = "SESSHAUVDEV3" +@load "liljon17.jld" +# include(joinpath(dirname(@__FILE__),"..","database","blandauthremote.jl")) +# user_config = addrdict +backend_config, user_config = standardcloudgraphsetup(addrdict=user_config) + + +println("[Caesar.jl] Setting up local solver") +slam_client = initialize!(backend_config,user_config) + +# create new handlers to pass in additional data +lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) +lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) +lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) +lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) +lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) + +# create LCM object and subscribe to messages on the following channels +lcm_node = LCM() +# poses +subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler) +# factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler) +subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler) +subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler) # loop closures come in via p3p3nh factors +# sensor data +subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler) + +println("[Caesar.jl] Running LCM listener") +listener!(slam_client, lcm_node) From b324f03a8fb2f3da599e363af98f77f169364e79 Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 18 Apr 2018 12:45:26 -0400 Subject: [PATCH 02/27] work in progress --- examples/lcmserver/synchrony.jl | 156 ++++++++++++++++++++++++++++++-- 1 file changed, 147 insertions(+), 9 deletions(-) diff --git a/examples/lcmserver/synchrony.jl b/examples/lcmserver/synchrony.jl index 8ea3f2ad1..b0f2ca8a1 100644 --- a/examples/lcmserver/synchrony.jl +++ b/examples/lcmserver/synchrony.jl @@ -4,25 +4,156 @@ LCM Server: an LCM interface to Caesar.jl =# using Caesar, SynchronySDK +using Unmarshal using CaesarLCMTypes using TransformUtils, Rotations, CoordinateTransformations using Distributions using LCMCore using LibBSON -# using CloudGraphs # for sorryPedro + +using RobotTestDatasets + + +mutable struct CaesarSLAM + userId::AbstractString + robotId::AbstractString + sessionId::AbstractString + syncrconf::Union{Void, SynchronySDK.SynchronyConfig} + robot + session + + # Constructor + CaesarSLAM(; + userId::AbstractString="", + robotId::AbstractString="", + sessionId::AbstractString="", + syncrconf::Union{Void, SynchronySDK.SynchronyConfig}=nothing, + robot=nothing, + session=nothing + ) = new( + userId, + robotId, + sessionId, + syncrconf, + robot, + session + ) +end +""" +$(SIGNATURES) + +Get Synchrony configuration, default filepath location assumed as `~/Documents/synchronyConfig.json`. +""" +function loadSyncrConfig(; + filepath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") + ) where {AS <: AbstractString} + # + println(" - Retrieving Synchrony Configuration...") + configFile = open(filepath) + configData = JSON.parse(readstring(configFile)) + close(configFile) + Unmarshal.unmarshal(SynchronyConfig, configData) # synchronyConfig = +end -# ... Change all functions to use Synchrony instead +""" +$(SIGNATURES) -function initialize!(backend_config, - user_config) - println("[Caesar.jl] Setting up factor graph") - fg = Caesar.initfg(sessionname=user_config["session"], cloudgraph=backend_config) - println("[Caesar.jl] Creating SLAM client/object") - return SLAMWrapper(fg, nothing, 0) +Confirm that the robot already exists, create if it doesn't. +""" +function syncrRobot( + synchronyConfig::SynchronyConfig, + robotId::AS + ) where {AS <: AbstractString} + # + println(" - Creating or retrieving robot '$robotId'...") + robot = nothing + if(SynchronySDK.isRobotExisting(synchronyConfig, robotId)) + println(" -- Robot '$robotId' already exists, retrieving it...") + robot = getRobot(synchronyConfig, robotId) + else + # Create a new one + println(" -- Robot '$robotId' doesn't exist, creating it...") + newRobot = RobotRequest(robotId, "My New Bot", "Description of my neat robot", "Active") + robot = createRobot(synchronyConfig, newRobot) + end + println(robot) + robot end +""" +$(SIGNATURES) + +Get sessions, if it already exists, add to it. +""" +function syncrSession( + synchronyConfig::SynchronyConfig, + robotId::AS, + sessionId::AS + ) where {AS<: AbstractString} + # + println(" - Creating or retrieving data session '$sessionId' for robot...") + session = nothing + if(SynchronySDK.isSessionExisting(synchronyConfig, robotId, sessionId)) + println(" -- Session '$sessionId' already exists for robot '$robotId', retrieving it...") + session = getSession(synchronyConfig, robotId, sessionId) + else + # Create a new one + println(" -- Session '$sessionId' doesn't exist for robot '$robotId', creating it...") + newSessionRequest = SessionDetailsRequest(sessionId, "A test dataset demonstrating data ingestion for a wheeled vehicle driving in a hexagon.") + session = createSession(synchronyConfig, robotId, newSessionRequest) + end + println(session) + session +end + +""" +$(SIGNATURES) + +Intialize the `cslaml` object using configuration file defined in `syncrconfpath`. +""" +function initialize!(cslaml::CaesarSLAM; + syncrconfpath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") + ) where {AS <: AbstractString} + # 1. Get a Synchrony configuration + cslaml.syncrconf = loadSyncrConfig(filepath=syncrconfpath) + + # 2. Confirm that the robot already exists, create if it doesn't. + cslaml.robot = syncrRobot(cslaml.syncrconf, cslaml.robotId) + + # 3. Create or retrieve the session. + cslaml.session = syncrSession(cslaml.syncrconf, cslaml.robotId, cslaml.sessionId) + + nothing +end + + +# create a SLAM container object +cslam = CaesarSLAM(userId=userId,robotId=robotId,sessionId=sessionId) + +# initialize a new session ready for SLAM using the built in SynchronySDK +initialize!(cslam) + + +# TEMPORARY CHECK THAT THNGS ARE WORKING (albeit 2D / need 3D) +for i in 0:5 + deltaMeasurement = [10.0;0;pi/3] + pOdo = Float64[[0.1 0 0] [0 0.1 0] [0 0 0.1]] + println(" - Measurement $i: Adding new odometry measurement '$deltaMeasurement'...") + newOdometryMeasurement = AddOdometryRequest(deltaMeasurement, pOdo) + @time odoResponse = addOdometryMeasurement(cslam.syncrconf, cslam.robotId, cslam.sessionId, newOdometryMeasurement) +end + + +# TODO -- MUST UPDATE Synchr to allow 3D and not auto create x0 with prior since 2D or 3D are very different + + + +# OLD CLOUD GRAPHS CODE STILL TO BE CONVERTED BELOW + + + function setRobotParameters!(cg::CloudGraph, session::AbstractString) hauvconfig = Dict() hauvconfig["robot"] = "hauv" @@ -242,7 +373,14 @@ function listener!(slam::SLAMWrapper, end -#println("[Caesar.jl] Prompting user for configuration") +# 0. Constants +println("[Caesar.jl] defining constants.") +data = robotdata("rovlcm_singlesession_01") +userId = "ROVUser" +robotId = "HAUV" +sessionId = "LCM_01" + + #@load "usercfg.jld" #user_config["session"] = "SESSHAUVDEV3" @load "liljon17.jld" From cec8bfe37c94b5b457e73e2ed63b162722e033ad Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 18 Apr 2018 14:33:37 -0400 Subject: [PATCH 03/27] mostly reworked hauv example but Syncr has not been updated yet, code not tested and waiting on Syncr development before more work can continue on this file --- examples/lcmserver/synchrony.jl | 221 ++++++++++++++++---------------- 1 file changed, 108 insertions(+), 113 deletions(-) diff --git a/examples/lcmserver/synchrony.jl b/examples/lcmserver/synchrony.jl index b0f2ca8a1..b0a7d4596 100644 --- a/examples/lcmserver/synchrony.jl +++ b/examples/lcmserver/synchrony.jl @@ -13,7 +13,11 @@ using LibBSON using RobotTestDatasets +""" + CaesarSLAM +An object definition containing the require variables to leverage the server side SLAM solution per user, robot, and session. +""" mutable struct CaesarSLAM userId::AbstractString robotId::AbstractString @@ -129,32 +133,15 @@ function initialize!(cslaml::CaesarSLAM; end -# create a SLAM container object -cslam = CaesarSLAM(userId=userId,robotId=robotId,sessionId=sessionId) - -# initialize a new session ready for SLAM using the built in SynchronySDK -initialize!(cslam) - - -# TEMPORARY CHECK THAT THNGS ARE WORKING (albeit 2D / need 3D) -for i in 0:5 - deltaMeasurement = [10.0;0;pi/3] - pOdo = Float64[[0.1 0 0] [0 0.1 0] [0 0 0.1]] - println(" - Measurement $i: Adding new odometry measurement '$deltaMeasurement'...") - newOdometryMeasurement = AddOdometryRequest(deltaMeasurement, pOdo) - @time odoResponse = addOdometryMeasurement(cslam.syncrconf, cslam.robotId, cslam.sessionId, newOdometryMeasurement) -end - - -# TODO -- MUST UPDATE Synchr to allow 3D and not auto create x0 with prior since 2D or 3D are very different - - - -# OLD CLOUD GRAPHS CODE STILL TO BE CONVERTED BELOW +# TODO -- code below untested +""" + setRobotParameters!(::CaesarSLAM) -function setRobotParameters!(cg::CloudGraph, session::AbstractString) +Store robot parameters in the centralized database system. +""" +function setRobotParameters!(cslaml::CaesarSLAM) hauvconfig = Dict() hauvconfig["robot"] = "hauv" hauvconfig["bTc"] = [0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0] @@ -170,16 +157,18 @@ function setRobotParameters!(cg::CloudGraph, session::AbstractString) end """ + handle_poses!(::CaesarSLAM, ::pose_node_t) + Adds pose nodes to graph with a prior on Z, pitch, and roll. """ -function handle_poses!(slam::SLAMWrapper, - message_data) - message = caesar.pose_node_t[:decode](message_data) - id = message[:id] - println("[Caesar.jl] Received pose message for x$(id)") - - mean = message[:mean] - covar = message[:covar] +function handle_poses!(slaml::CaesarSLAM, + msg::pose_node_t) + # + id = msg.id + println("[Caesar.jl] Received pose msg for x$(id)") + + mean = msg.mean + covar = msg.covar t = [mean[1], mean[2], mean[3]] qw = mean[4] qxyz = [mean[5], mean[6], mean[7]] @@ -188,102 +177,110 @@ function handle_poses!(slam::SLAMWrapper, euler = Euler(q) node_label = Symbol("x$(id)") - xn = addNode!(slam.fg, node_label, labels=["POSE"], dims=6) # this is an incremental inference call - slam.lastposesym = node_label; # update object + xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call + slaml.lastposesym = node_label; # update object if id == 1 println("[Caesar.jl] First pose") - # this is the first message, and it does not carry odometry, but the prior on the first node. + # this is the first msg, and it does not carry odometry, but the prior on the first node. # add 6dof prior initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) - addFactor!(slam.fg, [xn], initPosePrior) + addFactor!(slaml, [xn], initPosePrior) # auto init is coming, this code will likely be removed - initializeNode!(slam.fg, node_label) + initializeNode!(slaml, node_label) # set robot parameters in the first pose, this will become a separate node in the future println("[Caesar.jl] Setting robot parameters") - setRobotParameters!(slam.fg.cg, slam.fg.sessionname) + setRobotParameters!(slaml) end end -# handles ZPR priors on poses -function handle_priors!(slam::SLAMWrapper, - message_data) +""" + handle_priors!(::CaesarSLAM, ::prior_zpr_t) +Handle ZPR priors on poses. +""" +function handle_priors!(slam::CaesarSLAM, + msg::prior_zpr_t) - message = caesar.prior_zpr_t[:decode](message_data) - id = message[:id] + id = msg.id println("[Caesar.jl] Adding prior on RPZ to x$(id)") node_label = Symbol("x$(id)") - xn = getVert(slam.fg,node_label) + xn = getVert(slam,node_label) - z = message[:z] - pitch = message[:pitch] - roll = message[:roll] + z = msg.z + pitch = msg.pitch + roll = msg.roll - var_z = message[:var_z] - var_pitch = message[:var_pitch] - var_roll = message[:var_roll] + var_z = msg.var_z + var_pitch = msg.var_pitch + var_roll = msg.var_roll rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) z_dist = Normal(z, var_z) prior_rpz = PartialPriorRollPitchZ(rp_dist, z_dist) - addFactor!(slam.fg, [xn], prior_rpz) + addFactor!(slam, [xn], prior_rpz) end +""" + handle_partials!(::CaesarSLAM, ::pose_pose_xyh_t) -function handle_partials!(slam::SLAMWrapper, - message_data) - message = caesar.pose_pose_xyh_t[:decode](message_data) +Handle partial x, y, and heading odometry constraints between Pose3 variables. +""" +function handle_partials!(slam::CaesarSLAM, + msg::pose_pose_xyh_t) - origin_id = message[:node_1_id] - destination_id = message[:node_2_id] + origin_id = msg.node_1_id + destination_id = msg.node_2_id origin_label = Symbol("x$(origin_id)") destination_label = Symbol("x$(destination_id)") println("[Caesar.jl] Adding XYH odometry constraint between(x$(origin_id), x$(destination_id))") - delta_x = message[:delta_x] - delta_y = message[:delta_y] - delta_yaw = message[:delta_yaw] + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw - var_x = message[:var_x] - var_y = message[:var_y] - var_yaw = message[:var_yaw] + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw origin_label, destination_label - xo = getVert(slam.fg,origin_label) - xd = getVert(slam.fg,destination_label) + xo = getVert(slam,origin_label) + xd = getVert(slam,destination_label) xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) xyh_factor = PartialPose3XYYaw(xyh_dist) - addFactor!(slam.fg, [xo;xd], xyh_factor) + addFactor!(slam, [xo;xd], xyh_factor) - initializeNode!(slam.fg, destination_label) + initializeNode!(slam, destination_label) println() end +""" + handle_loops!(::CaesarSLAM, ::pose_pose_xyh_nh_t) -function handle_loops!(slaml::SLAMWrapper, - message_data) - message = caesar.pose_pose_xyh_nh_t[:decode](message_data) +Handle loop closure proposals with chance of being a null hypothesis likelihood. +""" +function handle_loops!(slaml::CaesarSLAM, + msg::pose_pose_xyh_nh_t) - origin_id = message[:node_1_id] - destination_id = message[:node_2_id] + origin_id = msg.node_1_id + destination_id = msg.node_2_id origin_label = Symbol("x$(origin_id)") destination_label = Symbol("x$(destination_id)") - delta_x = message[:delta_x] - delta_y = message[:delta_y] - delta_yaw = message[:delta_yaw] + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw - var_x = message[:var_x] - var_y = message[:var_y] - var_yaw = message[:var_yaw] - confidence = message[:confidence] + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + confidence = msg.confidence xo = getVert(slaml.fg,origin_label) xd = getVert(slaml.fg,destination_label) @@ -296,7 +293,7 @@ function handle_loops!(slaml::SLAMWrapper, println("[Caesar.jl] Adding XYH-NH loop closure constraint between (x$(origin_id), x$(destination_id))") xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) xyh_factor = PartialPose3XYYawNH(xyh_dist ,[1.0-confidence, confidence]) # change to NH - addFactor!(slaml.fg, [xo;xd], xyh_factor ) + addFactor!(slaml, [xo;xd], xyh_factor ) # println("[Caesar.jl] Adding P3P3NH loop closure constraint between (x$(origin_id), x$(destination_id))") @@ -304,7 +301,7 @@ function handle_loops!(slaml::SLAMWrapper, # lcf = Pose3Pose3NH( MvNormal(veeEuler(rel_pose), diagm(1.0./covar)), [0.5;0.5]) # define 50/50% hypothesis # lcf_label = Symbol[origin_label;destination_label] - # addFactor!(slaml.fg, lcf_label, lcf) + # addFactor!(slaml, lcf_label, lcf) end @@ -313,7 +310,7 @@ end # colors:: # end -# function add_pointcloud!(slaml::SLAMWrapper, nodeID::Symbol, cloud::Cloud ) +# function add_pointcloud!(slaml::CaesarSLAM, nodeID::Symbol, cloud::Cloud ) # # fetch from database # vert = getVert(slaml.fg, nodeID, api=IncrementalInference.dlapi) @@ -329,42 +326,39 @@ end """ - handle_clouds(slam::SLAMWrapper, message_data) + handle_clouds(slam::CaesarSLAM, msg_data) - Callback for caesar_point_cloud_t messages. Adds point cloud to SLAM_Client + Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client """ -function handle_clouds!(slam::SLAMWrapper, - message_data) +function handle_clouds!(slaml::CaesarSLAM, + msg::point_cloud_t) # TODO: interface here should be as simple as slam_client.add_pointcloud(nodeID, pc::SomeCloudType) # TODO: check for empty clouds! - message = caesar.point_cloud_t[:decode](message_data) - - id = message[:id] + id = msg.id last_pose = Symbol("x$(id)") println("[Caesar.jl] Got cloud $id") # 2d arrays of points and colors (from LCM data into arrays{arrays}) - points = [[pt[1], pt[2], pt[3]] for pt in message[:points]] - colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in message[:colors]] + points = [[pt[1], pt[2], pt[3]] for pt in msg.points] + colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in msg.colors] - # TODO: check if vert exists or not (may happen if messages are lost or out of order) - vert = getVert(slam.fg, last_pose, api=IncrementalInference.dlapi) # fetch from database + # TODO: check if vert exists or not (may happen if msgs are lost or out of order) + vert = getVert(slaml, last_pose, api=IncrementalInference.dlapi) # fetch from database # push to mongo (using BSON as a quick fix) # (for deserialization, see src/DirectorVisService.jl:cachepointclouds!) serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) - appendvertbigdata!(slam.fg, vert, "BSONpointcloud", string(serialized_point_cloud).data) + appendvertbigdata!(slaml, vert, "BSONpointcloud", string(serialized_point_cloud).data) serialized_colors = BSONObject(Dict("colors" => colors)) - appendvertbigdata!(slam.fg, vert, "BSONcolors", string(serialized_colors).data) + appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) end -# this function handles lcm messages -function listener!(slam::SLAMWrapper, - lcm_node::LCMCore.LCM) +# this function handles lcm msgs +function listener!(lcm_node::Union{LCMCore.LCM, LCMCore.LCMLog}) # handle traffic # TODO: handle termination while true @@ -375,22 +369,18 @@ end # 0. Constants println("[Caesar.jl] defining constants.") -data = robotdata("rovlcm_singlesession_01") userId = "ROVUser" robotId = "HAUV" sessionId = "LCM_01" +# create a SLAM container object +slam_client = CaesarSLAM(userId=userId,robotId=robotId,sessionId=sessionId) -#@load "usercfg.jld" -#user_config["session"] = "SESSHAUVDEV3" -@load "liljon17.jld" -# include(joinpath(dirname(@__FILE__),"..","database","blandauthremote.jl")) -# user_config = addrdict -backend_config, user_config = standardcloudgraphsetup(addrdict=user_config) - +# initialize a new session ready for SLAM using the built in SynchronySDK +println("[Caesar.jl] Setting up remote solver") +initialize!(slam_client) -println("[Caesar.jl] Setting up local solver") -slam_client = initialize!(backend_config,user_config) +# TODO - should have a function that allows first pose and prior to be set by user. # create new handlers to pass in additional data lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) @@ -400,15 +390,20 @@ lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, messa lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) # create LCM object and subscribe to messages on the following channels -lcm_node = LCM() +logfile = robotdata("rovlcm_singlesession_01") +lcm_node = LCMLog(logfile) # for direct log file access +# lcm_node = LCM() # for UDP Ethernet traffic version + # poses -subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler) +subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_pose_nh_t) # factors -subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler) -subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler) -subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler) # loop closures come in via p3p3nh factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) +subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +# loop closures come in via p3p3nh factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) # sensor data -subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler) +subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) + println("[Caesar.jl] Running LCM listener") -listener!(slam_client, lcm_node) +listener!(lcm_node) From edfb87c3cfefc5a2cbb6f8dd537e44191869bb7d Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 18 Apr 2018 15:12:48 -0400 Subject: [PATCH 04/27] cheange to SyncrSLAM --- examples/lcmserver/synchrony.jl | 36 ++++++++++++++++----------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/examples/lcmserver/synchrony.jl b/examples/lcmserver/synchrony.jl index b0a7d4596..82e29aa46 100644 --- a/examples/lcmserver/synchrony.jl +++ b/examples/lcmserver/synchrony.jl @@ -14,11 +14,11 @@ using LibBSON using RobotTestDatasets """ - CaesarSLAM + SyncrSLAM An object definition containing the require variables to leverage the server side SLAM solution per user, robot, and session. """ -mutable struct CaesarSLAM +mutable struct SyncrSLAM userId::AbstractString robotId::AbstractString sessionId::AbstractString @@ -27,7 +27,7 @@ mutable struct CaesarSLAM session # Constructor - CaesarSLAM(; + SyncrSLAM(; userId::AbstractString="", robotId::AbstractString="", sessionId::AbstractString="", @@ -117,7 +117,7 @@ $(SIGNATURES) Intialize the `cslaml` object using configuration file defined in `syncrconfpath`. """ -function initialize!(cslaml::CaesarSLAM; +function initialize!(cslaml::SyncrSLAM; syncrconfpath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") ) where {AS <: AbstractString} # 1. Get a Synchrony configuration @@ -137,11 +137,11 @@ end # TODO -- code below untested """ - setRobotParameters!(::CaesarSLAM) + setRobotParameters!(::SyncrSLAM) Store robot parameters in the centralized database system. """ -function setRobotParameters!(cslaml::CaesarSLAM) +function setRobotParameters!(cslaml::SyncrSLAM) hauvconfig = Dict() hauvconfig["robot"] = "hauv" hauvconfig["bTc"] = [0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0] @@ -157,11 +157,11 @@ function setRobotParameters!(cslaml::CaesarSLAM) end """ - handle_poses!(::CaesarSLAM, ::pose_node_t) + handle_poses!(::SyncrSLAM, ::pose_node_t) Adds pose nodes to graph with a prior on Z, pitch, and roll. """ -function handle_poses!(slaml::CaesarSLAM, +function handle_poses!(slaml::SyncrSLAM, msg::pose_node_t) # id = msg.id @@ -198,11 +198,11 @@ function handle_poses!(slaml::CaesarSLAM, end """ - handle_priors!(::CaesarSLAM, ::prior_zpr_t) + handle_priors!(::SyncrSLAM, ::prior_zpr_t) Handle ZPR priors on poses. """ -function handle_priors!(slam::CaesarSLAM, +function handle_priors!(slam::SyncrSLAM, msg::prior_zpr_t) id = msg.id @@ -226,11 +226,11 @@ function handle_priors!(slam::CaesarSLAM, end """ - handle_partials!(::CaesarSLAM, ::pose_pose_xyh_t) + handle_partials!(::SyncrSLAM, ::pose_pose_xyh_t) Handle partial x, y, and heading odometry constraints between Pose3 variables. """ -function handle_partials!(slam::CaesarSLAM, +function handle_partials!(slam::SyncrSLAM, msg::pose_pose_xyh_t) origin_id = msg.node_1_id @@ -261,11 +261,11 @@ function handle_partials!(slam::CaesarSLAM, end """ - handle_loops!(::CaesarSLAM, ::pose_pose_xyh_nh_t) + handle_loops!(::SyncrSLAM, ::pose_pose_xyh_nh_t) Handle loop closure proposals with chance of being a null hypothesis likelihood. """ -function handle_loops!(slaml::CaesarSLAM, +function handle_loops!(slaml::SyncrSLAM, msg::pose_pose_xyh_nh_t) origin_id = msg.node_1_id @@ -310,7 +310,7 @@ end # colors:: # end -# function add_pointcloud!(slaml::CaesarSLAM, nodeID::Symbol, cloud::Cloud ) +# function add_pointcloud!(slaml::SyncrSLAM, nodeID::Symbol, cloud::Cloud ) # # fetch from database # vert = getVert(slaml.fg, nodeID, api=IncrementalInference.dlapi) @@ -326,11 +326,11 @@ end """ - handle_clouds(slam::CaesarSLAM, msg_data) + handle_clouds(slam::SyncrSLAM, msg_data) Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client """ -function handle_clouds!(slaml::CaesarSLAM, +function handle_clouds!(slaml::SyncrSLAM, msg::point_cloud_t) # TODO: interface here should be as simple as slam_client.add_pointcloud(nodeID, pc::SomeCloudType) @@ -374,7 +374,7 @@ robotId = "HAUV" sessionId = "LCM_01" # create a SLAM container object -slam_client = CaesarSLAM(userId=userId,robotId=robotId,sessionId=sessionId) +slam_client = SyncrSLAM(userId=userId,robotId=robotId,sessionId=sessionId) # initialize a new session ready for SLAM using the built in SynchronySDK println("[Caesar.jl] Setting up remote solver") From 3770e0ef509dcb75eb52bc36474a97e3877e5710 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 19 Apr 2018 14:42:48 -0400 Subject: [PATCH 05/27] work in progress --- examples/lcmserver/synchrony.jl | 57 +++++++++++++++++++-------------- 1 file changed, 33 insertions(+), 24 deletions(-) diff --git a/examples/lcmserver/synchrony.jl b/examples/lcmserver/synchrony.jl index 82e29aa46..4c1fe9117 100644 --- a/examples/lcmserver/synchrony.jl +++ b/examples/lcmserver/synchrony.jl @@ -10,9 +10,12 @@ using TransformUtils, Rotations, CoordinateTransformations using Distributions using LCMCore using LibBSON +using DocStringExtensions # temporary while $(SIGNATURES) is in use in this file using RobotTestDatasets +const Syncr = SynchronySDK + """ SyncrSLAM @@ -115,49 +118,53 @@ end """ $(SIGNATURES) -Intialize the `cslaml` object using configuration file defined in `syncrconfpath`. +Intialize the `sslaml` object using configuration file defined in `syncrconfpath`. """ -function initialize!(cslaml::SyncrSLAM; +function initialize!(sslaml::SyncrSLAM; syncrconfpath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") ) where {AS <: AbstractString} # 1. Get a Synchrony configuration - cslaml.syncrconf = loadSyncrConfig(filepath=syncrconfpath) + sslaml.syncrconf = loadSyncrConfig(filepath=syncrconfpath) # 2. Confirm that the robot already exists, create if it doesn't. - cslaml.robot = syncrRobot(cslaml.syncrconf, cslaml.robotId) + sslaml.robot = syncrRobot(sslaml.syncrconf, sslaml.robotId) # 3. Create or retrieve the session. - cslaml.session = syncrSession(cslaml.syncrconf, cslaml.robotId, cslaml.sessionId) + sslaml.session = syncrSession(sslaml.syncrconf, sslaml.robotId, sslaml.sessionId) nothing end - -# TODO -- code below untested - """ - setRobotParameters!(::SyncrSLAM) +$(SIGNATURES) Store robot parameters in the centralized database system. """ -function setRobotParameters!(cslaml::SyncrSLAM) - hauvconfig = Dict() - hauvconfig["robot"] = "hauv" - hauvconfig["bTc"] = [0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0] - hauvconfig["bTc_format"] = "xyzqwqxqyqz" +function setRobotParameters!(sslaml::SyncrSLAM) + rovconf = Dict{String, String}() # TODO relax to Dict{String, Any} + rovconf["robot"] = "hauv" + rovconf["bTc"] = "[0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0]" + rovconf["bTc_format"] = "xyzqwqxqyqz" # currently unused, but upcoming - hauvconfig["pointcloud_description_name"] = "BSONpointcloud" - hauvconfig["pointcloud_color_description_name"] = "BSONcolors" - # robotdata = json(hauvconfig).data + rovconf["pointcloud_description_name"] = "BSONpointcloud" + rovconf["pointcloud_color_description_name"] = "BSONcolors" # Actually modify the databases - insertrobotdatafirstpose!(cg, session, hauvconfig) + Syncr.updateRobotConfig(sslaml.syncrconf, sslaml.robotId, rovconf) + + # insertrobotdatafirstpose!(cg, session, hauvconfig) nothing end + + +# TODO -- code below untested + + + """ - handle_poses!(::SyncrSLAM, ::pose_node_t) +$(SIGNATURES) Adds pose nodes to graph with a prior on Z, pitch, and roll. """ @@ -198,7 +205,7 @@ function handle_poses!(slaml::SyncrSLAM, end """ - handle_priors!(::SyncrSLAM, ::prior_zpr_t) +$(SIGNATURES) Handle ZPR priors on poses. """ @@ -226,7 +233,7 @@ function handle_priors!(slam::SyncrSLAM, end """ - handle_partials!(::SyncrSLAM, ::pose_pose_xyh_t) +$(SIGNATURES) Handle partial x, y, and heading odometry constraints between Pose3 variables. """ @@ -261,7 +268,7 @@ function handle_partials!(slam::SyncrSLAM, end """ - handle_loops!(::SyncrSLAM, ::pose_pose_xyh_nh_t) +$(SIGNATURES) Handle loop closure proposals with chance of being a null hypothesis likelihood. """ @@ -326,9 +333,9 @@ end """ - handle_clouds(slam::SyncrSLAM, msg_data) +$(SIGNATURES) - Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client +Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client """ function handle_clouds!(slaml::SyncrSLAM, msg::point_cloud_t) @@ -380,6 +387,8 @@ slam_client = SyncrSLAM(userId=userId,robotId=robotId,sessionId=sessionId) println("[Caesar.jl] Setting up remote solver") initialize!(slam_client) +setRobotParameters!(sslaml::SyncrSLAM) + # TODO - should have a function that allows first pose and prior to be set by user. # create new handlers to pass in additional data From 7e06a93e57f188a6f92771c7a433b5bd577c22a7 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Thu, 19 Apr 2018 19:54:29 -0500 Subject: [PATCH 06/27] Working now --- examples/lcmserver/synchrony.jl | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/examples/lcmserver/synchrony.jl b/examples/lcmserver/synchrony.jl index 4c1fe9117..697f1d431 100644 --- a/examples/lcmserver/synchrony.jl +++ b/examples/lcmserver/synchrony.jl @@ -22,7 +22,6 @@ const Syncr = SynchronySDK An object definition containing the require variables to leverage the server side SLAM solution per user, robot, and session. """ mutable struct SyncrSLAM - userId::AbstractString robotId::AbstractString sessionId::AbstractString syncrconf::Union{Void, SynchronySDK.SynchronyConfig} @@ -376,18 +375,17 @@ end # 0. Constants println("[Caesar.jl] defining constants.") -userId = "ROVUser" -robotId = "HAUV" +robotId = "HAUVSam" sessionId = "LCM_01" # create a SLAM container object -slam_client = SyncrSLAM(userId=userId,robotId=robotId,sessionId=sessionId) +slam_client = SyncrSLAM(robotId=robotId,sessionId=sessionId) # initialize a new session ready for SLAM using the built in SynchronySDK println("[Caesar.jl] Setting up remote solver") initialize!(slam_client) -setRobotParameters!(sslaml::SyncrSLAM) +setRobotParameters!(slam_client) # TODO - should have a function that allows first pose and prior to be set by user. From 401350984883018f8b58425a2f6bb3d2f7c55945 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Thu, 19 Apr 2018 20:21:17 -0500 Subject: [PATCH 07/27] Changing name back --- examples/lcmserver/synchrony.jl | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/lcmserver/synchrony.jl b/examples/lcmserver/synchrony.jl index 697f1d431..e1d0155d7 100644 --- a/examples/lcmserver/synchrony.jl +++ b/examples/lcmserver/synchrony.jl @@ -375,7 +375,7 @@ end # 0. Constants println("[Caesar.jl] defining constants.") -robotId = "HAUVSam" +robotId = "HAUV" sessionId = "LCM_01" # create a SLAM container object @@ -386,6 +386,8 @@ println("[Caesar.jl] Setting up remote solver") initialize!(slam_client) setRobotParameters!(slam_client) +# TEST: Getting robotConfig +@show robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) # TODO - should have a function that allows first pose and prior to be set by user. From 21a94741e8dc10c29368cbbcb84f3d97a2c1a72a Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 23 Apr 2018 21:13:56 -0400 Subject: [PATCH 08/27] merge all changes from master into feature branch --- examples/lcmserver/synchrony.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/lcmserver/synchrony.jl b/examples/lcmserver/synchrony.jl index e1d0155d7..d68b91555 100644 --- a/examples/lcmserver/synchrony.jl +++ b/examples/lcmserver/synchrony.jl @@ -375,7 +375,7 @@ end # 0. Constants println("[Caesar.jl] defining constants.") -robotId = "HAUV" +robotId = "HROV" sessionId = "LCM_01" # create a SLAM container object From 24dbc0c7a5316bdf6f64558e7dae444d135cfd1c Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 29 Apr 2018 12:39:48 -0400 Subject: [PATCH 09/27] moving example file --- examples/{lcmserver => rov/highend}/synchrony.jl | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/{lcmserver => rov/highend}/synchrony.jl (100%) diff --git a/examples/lcmserver/synchrony.jl b/examples/rov/highend/synchrony.jl similarity index 100% rename from examples/lcmserver/synchrony.jl rename to examples/rov/highend/synchrony.jl From 88a369871a590e4a31400b4a4972b7ba7a592022 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 29 Apr 2018 12:45:54 -0400 Subject: [PATCH 10/27] wip --- examples/rov/highend/synchrony.jl | 140 +----------------- .../rov/highend/synchronySDKIntegration.jl | 124 ++++++++++++++++ 2 files changed, 128 insertions(+), 136 deletions(-) create mode 100644 examples/rov/highend/synchronySDKIntegration.jl diff --git a/examples/rov/highend/synchrony.jl b/examples/rov/highend/synchrony.jl index d68b91555..fdebbed81 100644 --- a/examples/rov/highend/synchrony.jl +++ b/examples/rov/highend/synchrony.jl @@ -3,146 +3,25 @@ LCM Server: an LCM interface to Caesar.jl =# -using Caesar, SynchronySDK +using Caesar using Unmarshal using CaesarLCMTypes using TransformUtils, Rotations, CoordinateTransformations using Distributions using LCMCore using LibBSON -using DocStringExtensions # temporary while $(SIGNATURES) is in use in this file using RobotTestDatasets -const Syncr = SynchronySDK - -""" - SyncrSLAM - -An object definition containing the require variables to leverage the server side SLAM solution per user, robot, and session. -""" -mutable struct SyncrSLAM - robotId::AbstractString - sessionId::AbstractString - syncrconf::Union{Void, SynchronySDK.SynchronyConfig} - robot - session - - # Constructor - SyncrSLAM(; - userId::AbstractString="", - robotId::AbstractString="", - sessionId::AbstractString="", - syncrconf::Union{Void, SynchronySDK.SynchronyConfig}=nothing, - robot=nothing, - session=nothing - ) = new( - userId, - robotId, - sessionId, - syncrconf, - robot, - session - ) -end - - -""" -$(SIGNATURES) - -Get Synchrony configuration, default filepath location assumed as `~/Documents/synchronyConfig.json`. -""" -function loadSyncrConfig(; - filepath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") - ) where {AS <: AbstractString} - # - println(" - Retrieving Synchrony Configuration...") - configFile = open(filepath) - configData = JSON.parse(readstring(configFile)) - close(configFile) - Unmarshal.unmarshal(SynchronyConfig, configData) # synchronyConfig = -end - -""" -$(SIGNATURES) - -Confirm that the robot already exists, create if it doesn't. -""" -function syncrRobot( - synchronyConfig::SynchronyConfig, - robotId::AS - ) where {AS <: AbstractString} - # - println(" - Creating or retrieving robot '$robotId'...") - robot = nothing - if(SynchronySDK.isRobotExisting(synchronyConfig, robotId)) - println(" -- Robot '$robotId' already exists, retrieving it...") - robot = getRobot(synchronyConfig, robotId) - else - # Create a new one - println(" -- Robot '$robotId' doesn't exist, creating it...") - newRobot = RobotRequest(robotId, "My New Bot", "Description of my neat robot", "Active") - robot = createRobot(synchronyConfig, newRobot) - end - println(robot) - robot -end - -""" -$(SIGNATURES) - -Get sessions, if it already exists, add to it. -""" -function syncrSession( - synchronyConfig::SynchronyConfig, - robotId::AS, - sessionId::AS - ) where {AS<: AbstractString} - # - println(" - Creating or retrieving data session '$sessionId' for robot...") - session = nothing - if(SynchronySDK.isSessionExisting(synchronyConfig, robotId, sessionId)) - println(" -- Session '$sessionId' already exists for robot '$robotId', retrieving it...") - session = getSession(synchronyConfig, robotId, sessionId) - else - # Create a new one - println(" -- Session '$sessionId' doesn't exist for robot '$robotId', creating it...") - newSessionRequest = SessionDetailsRequest(sessionId, "A test dataset demonstrating data ingestion for a wheeled vehicle driving in a hexagon.") - session = createSession(synchronyConfig, robotId, newSessionRequest) - end - println(session) - session -end - -""" -$(SIGNATURES) - -Intialize the `sslaml` object using configuration file defined in `syncrconfpath`. -""" -function initialize!(sslaml::SyncrSLAM; - syncrconfpath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") - ) where {AS <: AbstractString} - # 1. Get a Synchrony configuration - sslaml.syncrconf = loadSyncrConfig(filepath=syncrconfpath) - - # 2. Confirm that the robot already exists, create if it doesn't. - sslaml.robot = syncrRobot(sslaml.syncrconf, sslaml.robotId) - - # 3. Create or retrieve the session. - sslaml.session = syncrSession(sslaml.syncrconf, sslaml.robotId, sslaml.sessionId) - - nothing -end +include(joinpath(dirname(@__FILE__), "synchronySDKIntegration.jl")) """ -$(SIGNATURES) - Store robot parameters in the centralized database system. """ function setRobotParameters!(sslaml::SyncrSLAM) rovconf = Dict{String, String}() # TODO relax to Dict{String, Any} - rovconf["robot"] = "hauv" + rovconf["robot"] = "hovering-rov" rovconf["bTc"] = "[0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0]" rovconf["bTc_format"] = "xyzqwqxqyqz" # currently unused, but upcoming @@ -152,19 +31,13 @@ function setRobotParameters!(sslaml::SyncrSLAM) # Actually modify the databases Syncr.updateRobotConfig(sslaml.syncrconf, sslaml.robotId, rovconf) - # insertrobotdatafirstpose!(cg, session, hauvconfig) nothing end - # TODO -- code below untested - - """ -$(SIGNATURES) - Adds pose nodes to graph with a prior on Z, pitch, and roll. """ function handle_poses!(slaml::SyncrSLAM, @@ -203,9 +76,8 @@ function handle_poses!(slaml::SyncrSLAM, end end -""" -$(SIGNATURES) +""" Handle ZPR priors on poses. """ function handle_priors!(slam::SyncrSLAM, @@ -232,8 +104,6 @@ function handle_priors!(slam::SyncrSLAM, end """ -$(SIGNATURES) - Handle partial x, y, and heading odometry constraints between Pose3 variables. """ function handle_partials!(slam::SyncrSLAM, @@ -267,8 +137,6 @@ function handle_partials!(slam::SyncrSLAM, end """ -$(SIGNATURES) - Handle loop closure proposals with chance of being a null hypothesis likelihood. """ function handle_loops!(slaml::SyncrSLAM, diff --git a/examples/rov/highend/synchronySDKIntegration.jl b/examples/rov/highend/synchronySDKIntegration.jl new file mode 100644 index 000000000..deab961cc --- /dev/null +++ b/examples/rov/highend/synchronySDKIntegration.jl @@ -0,0 +1,124 @@ +# SynchronySDK integration file + +using SynchronySDK +using DocStringExtensions # temporary while $(SIGNATURES) is in use in this file + +const Syncr = SynchronySDK + +""" + SyncrSLAM + +An object definition containing the require variables to leverage the server side SLAM solution per user, robot, and session. +""" +mutable struct SyncrSLAM + robotId::AbstractString + sessionId::AbstractString + syncrconf::Union{Void, SynchronySDK.SynchronyConfig} + robot + session + + # Constructor + SyncrSLAM(; + userId::AbstractString="", + robotId::AbstractString="", + sessionId::AbstractString="", + syncrconf::Union{Void, SynchronySDK.SynchronyConfig}=nothing, + robot=nothing, + session=nothing + ) = new( + userId, + robotId, + sessionId, + syncrconf, + robot, + session + ) +end + + +""" +$(SIGNATURES) + +Get Synchrony configuration, default filepath location assumed as `~/Documents/synchronyConfig.json`. +""" +function loadSyncrConfig(; + filepath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") + ) where {AS <: AbstractString} + # + println(" - Retrieving Synchrony Configuration...") + configFile = open(filepath) + configData = JSON.parse(readstring(configFile)) + close(configFile) + Unmarshal.unmarshal(SynchronyConfig, configData) # synchronyConfig = +end + +""" +$(SIGNATURES) + +Confirm that the robot already exists, create if it doesn't. +""" +function syncrRobot( + synchronyConfig::SynchronyConfig, + robotId::AS + ) where {AS <: AbstractString} + # + println(" - Creating or retrieving robot '$robotId'...") + robot = nothing + if(SynchronySDK.isRobotExisting(synchronyConfig, robotId)) + println(" -- Robot '$robotId' already exists, retrieving it...") + robot = getRobot(synchronyConfig, robotId) + else + # Create a new one + println(" -- Robot '$robotId' doesn't exist, creating it...") + newRobot = RobotRequest(robotId, "My New Bot", "Description of my neat robot", "Active") + robot = createRobot(synchronyConfig, newRobot) + end + println(robot) + robot +end + +""" +$(SIGNATURES) + +Get sessions, if it already exists, add to it. +""" +function syncrSession( + synchronyConfig::SynchronyConfig, + robotId::AS, + sessionId::AS + ) where {AS<: AbstractString} + # + println(" - Creating or retrieving data session '$sessionId' for robot...") + session = nothing + if(SynchronySDK.isSessionExisting(synchronyConfig, robotId, sessionId)) + println(" -- Session '$sessionId' already exists for robot '$robotId', retrieving it...") + session = getSession(synchronyConfig, robotId, sessionId) + else + # Create a new one + println(" -- Session '$sessionId' doesn't exist for robot '$robotId', creating it...") + newSessionRequest = SessionDetailsRequest(sessionId, "A test dataset demonstrating data ingestion for a wheeled vehicle driving in a hexagon.") + session = createSession(synchronyConfig, robotId, newSessionRequest) + end + println(session) + session +end + +""" +$(SIGNATURES) + +Intialize the `sslaml` object using configuration file defined in `syncrconfpath`. +""" +function initialize!(sslaml::SyncrSLAM; + syncrconfpath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") + ) where {AS <: AbstractString} + # 1. Get a Synchrony configuration + sslaml.syncrconf = loadSyncrConfig(filepath=syncrconfpath) + + # 2. Confirm that the robot already exists, create if it doesn't. + sslaml.robot = syncrRobot(sslaml.syncrconf, sslaml.robotId) + + # 3. Create or retrieve the session. + sslaml.session = syncrSession(sslaml.syncrconf, sslaml.robotId, sslaml.sessionId) + + nothing +end From e639f3dc369ec98c2c35c4e7d477da020eb056a2 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 29 Apr 2018 12:46:29 -0400 Subject: [PATCH 11/27] oops --- examples/rov/highend/synchrony.jl | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/rov/highend/synchrony.jl b/examples/rov/highend/synchrony.jl index fdebbed81..7bf85471d 100644 --- a/examples/rov/highend/synchrony.jl +++ b/examples/rov/highend/synchrony.jl @@ -200,8 +200,6 @@ end """ -$(SIGNATURES) - Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client """ function handle_clouds!(slaml::SyncrSLAM, From 417e58f3e18ec6fb3d817230bf1f4989d59eab44 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 29 Apr 2018 12:48:57 -0400 Subject: [PATCH 12/27] moving example files to better directories --- examples/{ => marine}/auv/Sandshark/Sandshark.jl | 0 examples/{ => marine}/rov/asfm/ASfMExample.jl | 0 examples/{ => marine}/rov/asfm/LRBEroll45deg.jl | 0 examples/{ => marine}/rov/asfm/LRBEyaw90deg.jl | 0 examples/{ => marine}/rov/asfm/LRBEzpitch45deg.jl | 0 examples/{ => marine}/rov/asfm/animateSonarSim.jl | 0 examples/{ => marine/rov/highend}/lcmserver/.gitignore | 0 examples/{ => marine/rov/highend}/lcmserver/README.md | 0 examples/{ => marine/rov/highend}/lcmserver/data.txt | 0 examples/{ => marine/rov/highend}/lcmserver/drawtoolsfromdb.jl | 0 .../{ => marine/rov/highend}/lcmserver/hauvvisualizationdebug.jl | 0 examples/{ => marine/rov/highend}/lcmserver/lcmdbservice.jl | 0 examples/{ => marine/rov/highend}/lcmserver/lcmpose3server.jl | 0 examples/{ => marine/rov/highend}/lcmserver/pointclouddev.jl | 0 .../{ => marine/rov/highend}/lcmserver/resolvingPartialXYH.jl | 0 .../{ => marine/rov/highend}/lcmserver/resolvingPartialXYH2.jl | 0 examples/{ => marine/rov/highend}/lcmserver/server.jl | 0 examples/{ => marine/rov/highend}/lcmserver/server_debug.jl | 0 examples/{ => marine}/rov/highend/synchrony.jl | 0 examples/{ => marine}/rov/highend/synchronySDKIntegration.jl | 0 20 files changed, 0 insertions(+), 0 deletions(-) rename examples/{ => marine}/auv/Sandshark/Sandshark.jl (100%) rename examples/{ => marine}/rov/asfm/ASfMExample.jl (100%) rename examples/{ => marine}/rov/asfm/LRBEroll45deg.jl (100%) rename examples/{ => marine}/rov/asfm/LRBEyaw90deg.jl (100%) rename examples/{ => marine}/rov/asfm/LRBEzpitch45deg.jl (100%) rename examples/{ => marine}/rov/asfm/animateSonarSim.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/.gitignore (100%) rename examples/{ => marine/rov/highend}/lcmserver/README.md (100%) rename examples/{ => marine/rov/highend}/lcmserver/data.txt (100%) rename examples/{ => marine/rov/highend}/lcmserver/drawtoolsfromdb.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/hauvvisualizationdebug.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/lcmdbservice.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/lcmpose3server.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/pointclouddev.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/resolvingPartialXYH.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/resolvingPartialXYH2.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/server.jl (100%) rename examples/{ => marine/rov/highend}/lcmserver/server_debug.jl (100%) rename examples/{ => marine}/rov/highend/synchrony.jl (100%) rename examples/{ => marine}/rov/highend/synchronySDKIntegration.jl (100%) diff --git a/examples/auv/Sandshark/Sandshark.jl b/examples/marine/auv/Sandshark/Sandshark.jl similarity index 100% rename from examples/auv/Sandshark/Sandshark.jl rename to examples/marine/auv/Sandshark/Sandshark.jl diff --git a/examples/rov/asfm/ASfMExample.jl b/examples/marine/rov/asfm/ASfMExample.jl similarity index 100% rename from examples/rov/asfm/ASfMExample.jl rename to examples/marine/rov/asfm/ASfMExample.jl diff --git a/examples/rov/asfm/LRBEroll45deg.jl b/examples/marine/rov/asfm/LRBEroll45deg.jl similarity index 100% rename from examples/rov/asfm/LRBEroll45deg.jl rename to examples/marine/rov/asfm/LRBEroll45deg.jl diff --git a/examples/rov/asfm/LRBEyaw90deg.jl b/examples/marine/rov/asfm/LRBEyaw90deg.jl similarity index 100% rename from examples/rov/asfm/LRBEyaw90deg.jl rename to examples/marine/rov/asfm/LRBEyaw90deg.jl diff --git a/examples/rov/asfm/LRBEzpitch45deg.jl b/examples/marine/rov/asfm/LRBEzpitch45deg.jl similarity index 100% rename from examples/rov/asfm/LRBEzpitch45deg.jl rename to examples/marine/rov/asfm/LRBEzpitch45deg.jl diff --git a/examples/rov/asfm/animateSonarSim.jl b/examples/marine/rov/asfm/animateSonarSim.jl similarity index 100% rename from examples/rov/asfm/animateSonarSim.jl rename to examples/marine/rov/asfm/animateSonarSim.jl diff --git a/examples/lcmserver/.gitignore b/examples/marine/rov/highend/lcmserver/.gitignore similarity index 100% rename from examples/lcmserver/.gitignore rename to examples/marine/rov/highend/lcmserver/.gitignore diff --git a/examples/lcmserver/README.md b/examples/marine/rov/highend/lcmserver/README.md similarity index 100% rename from examples/lcmserver/README.md rename to examples/marine/rov/highend/lcmserver/README.md diff --git a/examples/lcmserver/data.txt b/examples/marine/rov/highend/lcmserver/data.txt similarity index 100% rename from examples/lcmserver/data.txt rename to examples/marine/rov/highend/lcmserver/data.txt diff --git a/examples/lcmserver/drawtoolsfromdb.jl b/examples/marine/rov/highend/lcmserver/drawtoolsfromdb.jl similarity index 100% rename from examples/lcmserver/drawtoolsfromdb.jl rename to examples/marine/rov/highend/lcmserver/drawtoolsfromdb.jl diff --git a/examples/lcmserver/hauvvisualizationdebug.jl b/examples/marine/rov/highend/lcmserver/hauvvisualizationdebug.jl similarity index 100% rename from examples/lcmserver/hauvvisualizationdebug.jl rename to examples/marine/rov/highend/lcmserver/hauvvisualizationdebug.jl diff --git a/examples/lcmserver/lcmdbservice.jl b/examples/marine/rov/highend/lcmserver/lcmdbservice.jl similarity index 100% rename from examples/lcmserver/lcmdbservice.jl rename to examples/marine/rov/highend/lcmserver/lcmdbservice.jl diff --git a/examples/lcmserver/lcmpose3server.jl b/examples/marine/rov/highend/lcmserver/lcmpose3server.jl similarity index 100% rename from examples/lcmserver/lcmpose3server.jl rename to examples/marine/rov/highend/lcmserver/lcmpose3server.jl diff --git a/examples/lcmserver/pointclouddev.jl b/examples/marine/rov/highend/lcmserver/pointclouddev.jl similarity index 100% rename from examples/lcmserver/pointclouddev.jl rename to examples/marine/rov/highend/lcmserver/pointclouddev.jl diff --git a/examples/lcmserver/resolvingPartialXYH.jl b/examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl similarity index 100% rename from examples/lcmserver/resolvingPartialXYH.jl rename to examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl diff --git a/examples/lcmserver/resolvingPartialXYH2.jl b/examples/marine/rov/highend/lcmserver/resolvingPartialXYH2.jl similarity index 100% rename from examples/lcmserver/resolvingPartialXYH2.jl rename to examples/marine/rov/highend/lcmserver/resolvingPartialXYH2.jl diff --git a/examples/lcmserver/server.jl b/examples/marine/rov/highend/lcmserver/server.jl similarity index 100% rename from examples/lcmserver/server.jl rename to examples/marine/rov/highend/lcmserver/server.jl diff --git a/examples/lcmserver/server_debug.jl b/examples/marine/rov/highend/lcmserver/server_debug.jl similarity index 100% rename from examples/lcmserver/server_debug.jl rename to examples/marine/rov/highend/lcmserver/server_debug.jl diff --git a/examples/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl similarity index 100% rename from examples/rov/highend/synchrony.jl rename to examples/marine/rov/highend/synchrony.jl diff --git a/examples/rov/highend/synchronySDKIntegration.jl b/examples/marine/rov/highend/synchronySDKIntegration.jl similarity index 100% rename from examples/rov/highend/synchronySDKIntegration.jl rename to examples/marine/rov/highend/synchronySDKIntegration.jl From a099605f2eb6dab476b54c212911593369be7793 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 29 Apr 2018 16:27:06 -0400 Subject: [PATCH 13/27] moving examples --- examples/parameters/robots01.jl | 117 ++++++------- examples/rov/highend/synchrony.jl | 280 ++++++++++++++++++++++++++++++ 2 files changed, 339 insertions(+), 58 deletions(-) create mode 100644 examples/rov/highend/synchrony.jl diff --git a/examples/parameters/robots01.jl b/examples/parameters/robots01.jl index 4313fa76c..8dbd604eb 100644 --- a/examples/parameters/robots01.jl +++ b/examples/parameters/robots01.jl @@ -1,74 +1,75 @@ # example for loading robot description to database -using Caesar -using JSON -using Neo4j -using CloudGraphs +using Caesar, SynchronySDK -addrdict = nothing -include(joinpath(dirname(@__FILE__),"..","database","blandauthremote.jl")) -# prepare the factor graph with just one node -# (will prompt on stdin for db credentials) -cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict) - -# Turtlebot robot -turtlebot = Dict() -turtlebot["robot"] = "mrg-turtlebot" -turtlebot["CAMK"] = [ 570.34222412 0.0 319.5; - 0.0 570.34222412 239.5; - 0.0 0.0 1.0] -# -turtlebot["imshape"]=[480,640] -turtlebot["bTc"] = [0.0;0.0;0.6; 0.5; -0.5; 0.5; -0.5] -turtlebot["bTc_format"] = "xyzqwqxqyqz" - -# robotdata = json(turtlebot).data -# resp = JSON.parse(String(take!(IOBuffer(robotdata)))) -# tryunpackalltypes!(resp) -# resp - -# Actually modify the database -insertrobotdatafirstpose!(cloudGraph, "SESSTURT21", turtlebot) -frd = fetchrobotdatafirstpose(cloudGraph, "SESSTURT21") - - - - -# A different robot - -hauv = Dict() -hauv["robot"] = "hauv" -hauv["bTc"] = [0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0] -hauv["bTc_format"] = "xyzqwqxqyqz" -hauv["depth_pointcloud_description"] = "BSONpointcloud" -hauv["depth_color_description"] = ["BSONcolor"] - -# robotdata = json(hauv).data -hauv - +rovconf = Dict() +rovconf["robot"] = "hovering-rov" +rovconf["bTc"] = "[0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0]" +rovconf["bTc_format"] = "xyzqwqxqyqz" +rovconf["pointcloud_description_name"] = "BSONpointcloud" +rovconf["pointcloud_color_description_name"] = "BSONcolors" # Actually modify the databases -insertrobotdatafirstpose!(cloudGraph, "SESSHAUVDEV", hauv) -frd = fetchrobotdatafirstpose(cloudGraph, "SESSHAUVDEV") - - - - - - - - - - - +updateRobotConfig(synchronyConfig, robotId, rovconf) +## Obsolete usege below +# +# addrdict = nothing +# include(joinpath(dirname(@__FILE__),"..","database","blandauthremote.jl")) +# # prepare the factor graph with just one node +# # (will prompt on stdin for db credentials) +# cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict) +# +# +# +# # Turtlebot robot +# turtlebot = Dict() +# turtlebot["robot"] = "mrg-turtlebot" +# turtlebot["CAMK"] = [ 570.34222412 0.0 319.5; +# 0.0 570.34222412 239.5; +# 0.0 0.0 1.0] +# # +# turtlebot["imshape"]=[480,640] +# turtlebot["bTc"] = [0.0;0.0;0.6; 0.5; -0.5; 0.5; -0.5] +# turtlebot["bTc_format"] = "xyzqwqxqyqz" +# +# # robotdata = json(turtlebot).data +# # resp = JSON.parse(String(take!(IOBuffer(robotdata)))) +# # tryunpackalltypes!(resp) +# # resp +# +# # Actually modify the database +# insertrobotdatafirstpose!(cloudGraph, "SESSTURT21", turtlebot) +# frd = fetchrobotdatafirstpose(cloudGraph, "SESSTURT21") +# +# +# +# +# # A different robot +# +# hauv = Dict() +# hauv["robot"] = "hauv" +# hauv["bTc"] = [0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0] +# hauv["bTc_format"] = "xyzqwqxqyqz" +# hauv["depth_pointcloud_description"] = "BSONpointcloud" +# hauv["depth_color_description"] = ["BSONcolor"] +# +# # robotdata = json(hauv).data +# hauv +# +# +# # Actually modify the databases +# insertrobotdatafirstpose!(cloudGraph, "SESSHAUVDEV", hauv) +# frd = fetchrobotdatafirstpose(cloudGraph, "SESSHAUVDEV") +# +# diff --git a/examples/rov/highend/synchrony.jl b/examples/rov/highend/synchrony.jl new file mode 100644 index 000000000..40722e004 --- /dev/null +++ b/examples/rov/highend/synchrony.jl @@ -0,0 +1,280 @@ +#= +LCM Server: an LCM interface to Caesar.jl + +=# + +using Caesar +using LCMCore, CaesarLCMTypes +using TransformUtils, Rotations, CoordinateTransformations, Distributions +using LibBSON, Unmarshal + +using RobotTestDatasets + +# Does: using SynchronySDK +include(joinpath(dirname(@__FILE__), "synchronySDKIntegration.jl")) + + +""" +Store robot parameters in the centralized database system. +""" +function setRobotParameters!(sslaml::SyncrSLAM) + rovconf = Dict{String, String}() # TODO relax to Dict{String, Any} + rovconf["robot"] = "hovering-rov" + rovconf["bTc"] = "[0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0]" + rovconf["bTc_format"] = "xyzqwqxqyqz" + # currently unused, but upcoming + rovconf["pointcloud_description_name"] = "BSONpointcloud" + rovconf["pointcloud_color_description_name"] = "BSONcolors" + + # Actually modify the databases + Syncr.updateRobotConfig(sslaml.syncrconf, sslaml.robotId, rovconf) + + nothing +end + + +""" +Adds pose nodes to graph with a prior on Z, pitch, and roll. +""" +function handle_poses!(slaml::SyncrSLAM, + msg::pose_node_t) + # + id = msg.id + println("[Caesar.jl] Received pose msg for x$(id)") + + mean = msg.mean + covar = msg.covar + t = [mean[1], mean[2], mean[3]] + qw = mean[4] + qxyz = [mean[5], mean[6], mean[7]] + q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? + pose = SE3(t,q) + euler = Euler(q) + + node_label = Symbol("x$(id)") + xn = addNode!(slaml, node_label, Pose3) # this is an incremental inference call + slaml.lastposesym = node_label; # update object + + if id == 1 + println("[Caesar.jl] First pose") + # this is the first msg, and it does not carry odometry, but the prior on the first node. + + # add 6dof prior + initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) + addFactor!(slaml, [xn], initPosePrior) + + # auto init is coming, this code will likely be removed + initializeNode!(slaml, node_label) + + # set robot parameters in the first pose, this will become a separate node in the future + println("[Caesar.jl] Setting robot parameters") + setRobotParameters!(slaml) + end +end + + +""" +Handle ZPR priors on poses. +""" +function handle_priors!(slam::SyncrSLAM, + msg::prior_zpr_t) + + id = msg.id + println("[Caesar.jl] Adding prior on RPZ to x$(id)") + + node_label = Symbol("x$(id)") + xn = getVert(slam,node_label) + + z = msg.z + pitch = msg.pitch + roll = msg.roll + + var_z = msg.var_z + var_pitch = msg.var_pitch + var_roll = msg.var_roll + + rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) + z_dist = Normal(z, var_z) + prior_rpz = PartialPriorRollPitchZ(rp_dist, z_dist) + addFactor!(slam, [xn], prior_rpz) +end + +""" +Handle partial x, y, and heading odometry constraints between Pose3 variables. +""" +function handle_partials!(slam::SyncrSLAM, + msg::pose_pose_xyh_t) + + origin_id = msg.node_1_id + destination_id = msg.node_2_id + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + println("[Caesar.jl] Adding XYH odometry constraint between(x$(origin_id), x$(destination_id))") + + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw + + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + + origin_label, destination_label + xo = getVert(slam,origin_label) + xd = getVert(slam,destination_label) + + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = PartialPose3XYYaw(xyh_dist) + addFactor!(slam, [xo;xd], xyh_factor) + + initializeNode!(slam, destination_label) + println() +end + +""" +Handle loop closure proposals with chance of being a null hypothesis likelihood. +""" +function handle_loops!(slaml::SyncrSLAM, + msg::pose_pose_xyh_nh_t) + + origin_id = msg.node_1_id + destination_id = msg.node_2_id + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw + + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + confidence = msg.confidence + + xo = getVert(slaml.fg,origin_label) + xd = getVert(slaml.fg,destination_label) + + # if (destination_id - origin_id == 1) + # warn("Avoiding parallel factor! See: https://github.com/dehann/IncrementalInference.jl/issues/63To ") + # return + # end + + println("[Caesar.jl] Adding XYH-NH loop closure constraint between (x$(origin_id), x$(destination_id))") + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = PartialPose3XYYawNH(xyh_dist ,[1.0-confidence, confidence]) # change to NH + addFactor!(slaml, [xo;xd], xyh_factor ) + + # println("[Caesar.jl] Adding P3P3NH loop closure constraint between (x$(origin_id), x$(destination_id))") + + # # line below fails! + # lcf = Pose3Pose3NH( MvNormal(veeEuler(rel_pose), diagm(1.0./covar)), [0.5;0.5]) # define 50/50% hypothesis + # lcf_label = Symbol[origin_label;destination_label] + + # addFactor!(slaml, lcf_label, lcf) +end + + +# type Cloud +# points:: +# colors:: +# end + +# function add_pointcloud!(slaml::SyncrSLAM, nodeID::Symbol, cloud::Cloud ) +# # fetch from database +# vert = getVert(slaml.fg, nodeID, api=IncrementalInference.dlapi) + +# # add points blob +# serialized_point_cloud = BSONObject(Dict("pointcloud" => cloud.points)) +# appendvertbigdata!(slaml.fg, vert, "BSONpointcloud", string(serialized_point_cloud).data) + +# # add colors blob +# serialized_colors = BSONObject(Dict("colors" => cloud.colors)) +# appendvertbigdata!(slaml.fg, vert, "BSONcolors", string(serialized_colors).data) +# end + + + +""" +Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client +""" +function handle_clouds!(slaml::SyncrSLAM, + msg::point_cloud_t) + # TODO: interface here should be as simple as slam_client.add_pointcloud(nodeID, pc::SomeCloudType) + + # TODO: check for empty clouds! + + id = msg.id + + last_pose = Symbol("x$(id)") + println("[Caesar.jl] Got cloud $id") + + # 2d arrays of points and colors (from LCM data into arrays{arrays}) + points = [[pt[1], pt[2], pt[3]] for pt in msg.points] + colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in msg.colors] + + + # TODO: check if vert exists or not (may happen if msgs are lost or out of order) + vert = getVert(slaml, last_pose, api=IncrementalInference.dlapi) # fetch from database + + # push to mongo (using BSON as a quick fix) + # (for deserialization, see src/DirectorVisService.jl:cachepointclouds!) + serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) + appendvertbigdata!(slaml, vert, "BSONpointcloud", string(serialized_point_cloud).data) + serialized_colors = BSONObject(Dict("colors" => colors)) + appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) +end + +# this function handles lcm msgs +function listener!(lcm_node::Union{LCMCore.LCM, LCMCore.LCMLog}) + # handle traffic + # TODO: handle termination + while true + handle(lcm_node) + end +end + + +# 0. Constants +println("[Caesar.jl] defining constants.") +robotId = "HROV" +sessionId = "LCM_01" + +# create a SLAM container object +slam_client = SyncrSLAM(robotId=robotId,sessionId=sessionId) + +# initialize a new session ready for SLAM using the built in SynchronySDK +println("[Caesar.jl] Setting up remote solver") +initialize!(slam_client) + +setRobotParameters!(slam_client) +# TEST: Getting robotConfig +@show robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) + +# TODO - should have a function that allows first pose and prior to be set by user. + +# create new handlers to pass in additional data +lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) +lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) +lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) +lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) +lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) + +# create LCM object and subscribe to messages on the following channels +logfile = robotdata("rovlcm_singlesession_01") +lcm_node = LCMLog(logfile) # for direct log file access +# lcm_node = LCM() # for UDP Ethernet traffic version + +# poses +subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_pose_nh_t) +# factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) +subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +# loop closures come in via p3p3nh factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) +# sensor data +subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) + + +println("[Caesar.jl] Running LCM listener") +listener!(lcm_node) From 8830e927d44122d9963fec34e9f5ac0f40ed8800 Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 29 Apr 2018 16:33:04 -0400 Subject: [PATCH 14/27] wip --- examples/marine/rov/highend/synchrony.jl | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index 7bf85471d..5232d306e 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -271,13 +271,15 @@ lcm_node = LCMLog(logfile) # for direct log file access # poses subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_pose_nh_t) + # factors -subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) -subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +# subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) +# subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) # loop closures come in via p3p3nh factors -subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) +# subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) + # sensor data -subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) +# subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) println("[Caesar.jl] Running LCM listener") From ce040ed3f331bc6aca7aca49733d974dd828bee2 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 29 Apr 2018 17:25:55 -0500 Subject: [PATCH 15/27] Baseline --- examples/marine/rov/highend/synchrony.jl | 162 +++++++++--------- .../rov/highend/synchronySDKIntegration.jl | 14 +- 2 files changed, 86 insertions(+), 90 deletions(-) diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index 5232d306e..bc309da84 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -46,36 +46,92 @@ function handle_poses!(slaml::SyncrSLAM, id = msg.id println("[Caesar.jl] Received pose msg for x$(id)") - mean = msg.mean - covar = msg.covar - t = [mean[1], mean[2], mean[3]] - qw = mean[4] - qxyz = [mean[5], mean[6], mean[7]] - q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? - pose = SE3(t,q) - euler = Euler(q) + # mean = msg.mean + # covar = msg.covar + # t = [mean[1], mean[2], mean[3]] + # qw = mean[4] + # qxyz = [mean[5], mean[6], mean[7]] + # q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? + # pose = SE3(t,q) + # euler = Euler(q) + # + # node_label = Symbol("x$(id)") + # xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call + # slaml.lastposesym = node_label; # update object + # + # if id == 1 + # println("[Caesar.jl] First pose") + # # this is the first msg, and it does not carry odometry, but the prior on the first node. + # + # # add 6dof prior + # initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) + # addFactor!(slaml, [xn], initPosePrior) + # + # # auto init is coming, this code will likely be removed + # initializeNode!(slaml, node_label) + # + # # set robot parameters in the first pose, this will become a separate node in the future + # println("[Caesar.jl] Setting robot parameters") + # setRobotParameters!(slaml) + # end +end - node_label = Symbol("x$(id)") - xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call - slaml.lastposesym = node_label; # update object +# this function handles lcm msgs +function listener!(lcm_node::Union{LCMCore.LCM, LCMCore.LCMLog}) + # handle traffic + # TODO: handle termination + while true + handle(lcm_node) + end +end - if id == 1 - println("[Caesar.jl] First pose") - # this is the first msg, and it does not carry odometry, but the prior on the first node. - # add 6dof prior - initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) - addFactor!(slaml, [xn], initPosePrior) +# 0. Constants +println("[Caesar.jl] defining constants.") +robotId = "HROV" +sessionId = "LCM_01" - # auto init is coming, this code will likely be removed - initializeNode!(slaml, node_label) +# create a SLAM container object +slam_client = SyncrSLAM(robotId,sessionId, nothing) - # set robot parameters in the first pose, this will become a separate node in the future - println("[Caesar.jl] Setting robot parameters") - setRobotParameters!(slaml) - end -end +# initialize a new session ready for SLAM using the built in SynchronySDK +println("[Caesar.jl] Setting up remote solver") +initialize!(slam_client) + +setRobotParameters!(slam_client) +# TEST: Getting robotConfig +@show robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) + +# TODO - should have a function that allows first pose and prior to be set by user. + +# create new handlers to pass in additional data +lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) +# lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) +# lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) +# lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) +# lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) + +# create LCM object and subscribe to messages on the following channels +logfile = robotdata("rovlcm_singlesession_01") +lcm_node = LCMLog(logfile) # for direct log file access +# lcm_node = LCM() # for UDP Ethernet traffic version + +# poses +subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_pose_nh_t) + +# factors +# subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) +# subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +# loop closures come in via p3p3nh factors +# subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) + +# sensor data +# subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) + +println("[Caesar.jl] Running LCM listener") +listener!(lcm_node) +#### TODO stuff """ Handle ZPR priors on poses. @@ -197,8 +253,6 @@ end # appendvertbigdata!(slaml.fg, vert, "BSONcolors", string(serialized_colors).data) # end - - """ Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client """ @@ -228,59 +282,3 @@ function handle_clouds!(slaml::SyncrSLAM, serialized_colors = BSONObject(Dict("colors" => colors)) appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) end - -# this function handles lcm msgs -function listener!(lcm_node::Union{LCMCore.LCM, LCMCore.LCMLog}) - # handle traffic - # TODO: handle termination - while true - handle(lcm_node) - end -end - - -# 0. Constants -println("[Caesar.jl] defining constants.") -robotId = "HROV" -sessionId = "LCM_01" - -# create a SLAM container object -slam_client = SyncrSLAM(robotId=robotId,sessionId=sessionId) - -# initialize a new session ready for SLAM using the built in SynchronySDK -println("[Caesar.jl] Setting up remote solver") -initialize!(slam_client) - -setRobotParameters!(slam_client) -# TEST: Getting robotConfig -@show robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) - -# TODO - should have a function that allows first pose and prior to be set by user. - -# create new handlers to pass in additional data -lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) -lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) -lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) -lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) -lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) - -# create LCM object and subscribe to messages on the following channels -logfile = robotdata("rovlcm_singlesession_01") -lcm_node = LCMLog(logfile) # for direct log file access -# lcm_node = LCM() # for UDP Ethernet traffic version - -# poses -subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_pose_nh_t) - -# factors -# subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) -# subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) -# loop closures come in via p3p3nh factors -# subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) - -# sensor data -# subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) - - -println("[Caesar.jl] Running LCM listener") -listener!(lcm_node) diff --git a/examples/marine/rov/highend/synchronySDKIntegration.jl b/examples/marine/rov/highend/synchronySDKIntegration.jl index deab961cc..7e0918dc4 100644 --- a/examples/marine/rov/highend/synchronySDKIntegration.jl +++ b/examples/marine/rov/highend/synchronySDKIntegration.jl @@ -18,15 +18,13 @@ mutable struct SyncrSLAM session # Constructor - SyncrSLAM(; - userId::AbstractString="", - robotId::AbstractString="", - sessionId::AbstractString="", - syncrconf::Union{Void, SynchronySDK.SynchronyConfig}=nothing, + SyncrSLAM( + robotId::AbstractString, + sessionId::AbstractString, + syncrconf::Union{Void, SynchronySDK.SynchronyConfig}; robot=nothing, session=nothing ) = new( - userId, robotId, sessionId, syncrconf, @@ -71,7 +69,7 @@ function syncrRobot( # Create a new one println(" -- Robot '$robotId' doesn't exist, creating it...") newRobot = RobotRequest(robotId, "My New Bot", "Description of my neat robot", "Active") - robot = createRobot(synchronyConfig, newRobot) + robot = addRobot(synchronyConfig, newRobot) end println(robot) robot @@ -97,7 +95,7 @@ function syncrSession( # Create a new one println(" -- Session '$sessionId' doesn't exist for robot '$robotId', creating it...") newSessionRequest = SessionDetailsRequest(sessionId, "A test dataset demonstrating data ingestion for a wheeled vehicle driving in a hexagon.") - session = createSession(synchronyConfig, robotId, newSessionRequest) + session = addSession(synchronyConfig, robotId, newSessionRequest) end println(session) session From eeac62b6e8b490235a4c75ec8b3b90af48de2719 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 29 Apr 2018 17:30:56 -0500 Subject: [PATCH 16/27] Baseline --- examples/marine/rov/highend/synchrony.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index bc309da84..b748d8d80 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -117,7 +117,7 @@ lcm_node = LCMLog(logfile) # for direct log file access # lcm_node = LCM() # for UDP Ethernet traffic version # poses -subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_pose_nh_t) +subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_node_t) # factors # subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) From 4aad60b8bbf6a8ca79ee7f61b2c3d3b3013dd6c0 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sun, 29 Apr 2018 18:48:44 -0500 Subject: [PATCH 17/27] Pose additions working --- examples/marine/rov/highend/synchrony.jl | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index b748d8d80..ca1c5a50b 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -42,20 +42,21 @@ Adds pose nodes to graph with a prior on Z, pitch, and roll. """ function handle_poses!(slaml::SyncrSLAM, msg::pose_node_t) - # id = msg.id println("[Caesar.jl] Received pose msg for x$(id)") - # mean = msg.mean - # covar = msg.covar - # t = [mean[1], mean[2], mean[3]] - # qw = mean[4] - # qxyz = [mean[5], mean[6], mean[7]] - # q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? - # pose = SE3(t,q) - # euler = Euler(q) + mean = msg.mean + covar = msg.covar + t = [mean[1], mean[2], mean[3]] + qw = mean[4] + qxyz = [mean[5], mean[6], mean[7]] + q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? + pose = SE3(t,q) + euler = Euler(q) # - # node_label = Symbol("x$(id)") + node_label = Symbol("x$(id)") + varRequest = VariableRequest(node_label, "Pose3", nothing, ["POSE"]) + resp = addVariable(slaml.syncrconf, slaml.robotId, slaml.sessionId, varRequest) # xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call # slaml.lastposesym = node_label; # update object # From 8c1ff6080c764443c7a56c58e3ea7d4bb5d36fcb Mon Sep 17 00:00:00 2001 From: GearsAD Date: Mon, 30 Apr 2018 23:47:23 -0500 Subject: [PATCH 18/27] Factors close to completion --- examples/marine/rov/highend/synchrony.jl | 100 ++++++++++++----------- 1 file changed, 52 insertions(+), 48 deletions(-) diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index ca1c5a50b..424fb6e4f 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -60,21 +60,52 @@ function handle_poses!(slaml::SyncrSLAM, # xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call # slaml.lastposesym = node_label; # update object # - # if id == 1 - # println("[Caesar.jl] First pose") - # # this is the first msg, and it does not carry odometry, but the prior on the first node. - # - # # add 6dof prior - # initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) - # addFactor!(slaml, [xn], initPosePrior) - # - # # auto init is coming, this code will likely be removed - # initializeNode!(slaml, node_label) - # - # # set robot parameters in the first pose, this will become a separate node in the future - # println("[Caesar.jl] Setting robot parameters") - # setRobotParameters!(slaml) - # end + if id == 1 + println("[Caesar.jl] First pose") + # this is the first msg, and it does not carry odometry, but the prior on the first node. + + # add 6dof prior + initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) + addFactor!(slaml, [xn], initPosePrior) + + # auto init is coming, this code will likely be removed + initializeNode!(slaml, node_label) + + # set robot parameters in the first pose, this will become a separate node in the future + println("[Caesar.jl] Setting robot parameters") + setRobotParameters!(slaml) + end +end + +""" +Handle ZPR priors on poses. +""" +function handle_priors!(slam::SyncrSLAM, + msg::prior_zpr_t) + + id = msg.id + println("[Caesar.jl] Adding prior on RPZ to x$(id)") + + node_label = Symbol("x$(id)") + # xn = getVert(slam,node_label) + + z = msg.z + pitch = msg.pitch + roll = msg.roll + + var_z = msg.var_z + var_pitch = msg.var_pitch + var_roll = msg.var_roll + + rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) + z_dist = Normal(z, var_z) + prior_rpz = RoME.PartialPriorRollPitchZ(rp_dist, z_dist) + + # Build the factor request + fctBody = FactorBody(string(typeof(prior_rpz)), "JSON", JSON.json(prior_rpz)) + fctRequest = FactorRequest([node_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) + # addFactor!(slam, [xn], prior_rpz) end # this function handles lcm msgs @@ -90,7 +121,7 @@ end # 0. Constants println("[Caesar.jl] defining constants.") robotId = "HROV" -sessionId = "LCM_01" +sessionId = "LCM_09" # create a SLAM container object slam_client = SyncrSLAM(robotId,sessionId, nothing) @@ -100,15 +131,14 @@ println("[Caesar.jl] Setting up remote solver") initialize!(slam_client) setRobotParameters!(slam_client) -# TEST: Getting robotConfig -@show robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) +robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) # TODO - should have a function that allows first pose and prior to be set by user. # create new handlers to pass in additional data -lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) +# lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) # lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) -# lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) +lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) # lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) # lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) @@ -118,11 +148,11 @@ lcm_node = LCMLog(logfile) # for direct log file access # lcm_node = LCM() # for UDP Ethernet traffic version # poses -subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_node_t) +# subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_node_t) # factors # subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) -# subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) # loop closures come in via p3p3nh factors # subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) @@ -134,32 +164,6 @@ listener!(lcm_node) #### TODO stuff -""" -Handle ZPR priors on poses. -""" -function handle_priors!(slam::SyncrSLAM, - msg::prior_zpr_t) - - id = msg.id - println("[Caesar.jl] Adding prior on RPZ to x$(id)") - - node_label = Symbol("x$(id)") - xn = getVert(slam,node_label) - - z = msg.z - pitch = msg.pitch - roll = msg.roll - - var_z = msg.var_z - var_pitch = msg.var_pitch - var_roll = msg.var_roll - - rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) - z_dist = Normal(z, var_z) - prior_rpz = PartialPriorRollPitchZ(rp_dist, z_dist) - addFactor!(slam, [xn], prior_rpz) -end - """ Handle partial x, y, and heading odometry constraints between Pose3 variables. """ From 90a693cab10b40392dac5d8b45719d960376a911 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Tue, 1 May 2018 22:11:06 -0500 Subject: [PATCH 19/27] WIP, removing bugs on adding factors/priors --- examples/marine/rov/highend/synchrony.jl | 49 +++++++++++++------ .../rov/highend/synchronySDKIntegration.jl | 2 +- 2 files changed, 34 insertions(+), 17 deletions(-) diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index 424fb6e4f..17be3b709 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -40,7 +40,7 @@ end """ Adds pose nodes to graph with a prior on Z, pitch, and roll. """ -function handle_poses!(slaml::SyncrSLAM, +function handle_poses!(slam::SyncrSLAM, msg::pose_node_t) id = msg.id println("[Caesar.jl] Received pose msg for x$(id)") @@ -56,25 +56,32 @@ function handle_poses!(slaml::SyncrSLAM, # node_label = Symbol("x$(id)") varRequest = VariableRequest(node_label, "Pose3", nothing, ["POSE"]) - resp = addVariable(slaml.syncrconf, slaml.robotId, slaml.sessionId, varRequest) + resp = addVariable(slam.syncrconf, slam.robotId, slam.sessionId, varRequest) # xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call # slaml.lastposesym = node_label; # update object # - if id == 1 + if id == 0 println("[Caesar.jl] First pose") # this is the first msg, and it does not carry odometry, but the prior on the first node. # add 6dof prior - initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) - addFactor!(slaml, [xn], initPosePrior) + initPosePrior = RoME.PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) + packedPrior = convert(RoME.PackedPriorPose3, initPosePrior) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(initPosePrior)), string(typeof(packedPrior)), "JSON", JSON.json(packedPrior)) + fctRequest = FactorRequest([node_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) # auto init is coming, this code will likely be removed - initializeNode!(slaml, node_label) + # initializeNode!(slaml, node_label) # set robot parameters in the first pose, this will become a separate node in the future println("[Caesar.jl] Setting robot parameters") setRobotParameters!(slaml) end + + sleep(5) end """ @@ -89,23 +96,26 @@ function handle_priors!(slam::SyncrSLAM, node_label = Symbol("x$(id)") # xn = getVert(slam,node_label) + # 1. Build up the prior z = msg.z pitch = msg.pitch roll = msg.roll - var_z = msg.var_z var_pitch = msg.var_pitch var_roll = msg.var_roll - rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) z_dist = Normal(z, var_z) prior_rpz = RoME.PartialPriorRollPitchZ(rp_dist, z_dist) - # Build the factor request - fctBody = FactorBody(string(typeof(prior_rpz)), "JSON", JSON.json(prior_rpz)) + # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) + packed_prior_rpz = convert(RoME.PackedPartialPriorRollPitchZ, prior_rpz) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(prior_rpz)), string(typeof(packed_prior_rpz)), "JSON", JSON.json(packed_prior_rpz)) fctRequest = FactorRequest([node_label], fctBody, false, false) @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) - # addFactor!(slam, [xn], prior_rpz) + + sleep(5) end # this function handles lcm msgs @@ -121,22 +131,29 @@ end # 0. Constants println("[Caesar.jl] defining constants.") robotId = "HROV" -sessionId = "LCM_09" +sessionId = "LCM_13" # create a SLAM container object -slam_client = SyncrSLAM(robotId,sessionId, nothing) +slam_client = SyncrSLAM(robotId, sessionId, nothing) # initialize a new session ready for SLAM using the built in SynchronySDK println("[Caesar.jl] Setting up remote solver") initialize!(slam_client) +# Make sure that this session is not already populated +existingSessions = getSessions(slam_client.syncrconf, sessionId) +if count(session -> session.id == sessionId, existingSessions.sessions) > 0 + error("There is already a session named '$sessionId' for robot '$robotId'. This example will fail if it tries to add duplicate nodes. We strongly recommend providing a new session name.") +end + +# Set up the robot setRobotParameters!(slam_client) robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) # TODO - should have a function that allows first pose and prior to be set by user. # create new handlers to pass in additional data -# lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) +lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) # lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) # lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) @@ -145,10 +162,9 @@ lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, messa # create LCM object and subscribe to messages on the following channels logfile = robotdata("rovlcm_singlesession_01") lcm_node = LCMLog(logfile) # for direct log file access -# lcm_node = LCM() # for UDP Ethernet traffic version # poses -# subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_node_t) +subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_node_t) # factors # subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) @@ -162,6 +178,7 @@ subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) println("[Caesar.jl] Running LCM listener") listener!(lcm_node) + #### TODO stuff """ diff --git a/examples/marine/rov/highend/synchronySDKIntegration.jl b/examples/marine/rov/highend/synchronySDKIntegration.jl index 7e0918dc4..d55afa15f 100644 --- a/examples/marine/rov/highend/synchronySDKIntegration.jl +++ b/examples/marine/rov/highend/synchronySDKIntegration.jl @@ -94,7 +94,7 @@ function syncrSession( else # Create a new one println(" -- Session '$sessionId' doesn't exist for robot '$robotId', creating it...") - newSessionRequest = SessionDetailsRequest(sessionId, "A test dataset demonstrating data ingestion for a wheeled vehicle driving in a hexagon.") + newSessionRequest = SessionDetailsRequest(sessionId, "Submersible vehicle.") session = addSession(synchronyConfig, robotId, newSessionRequest) end println(session) From 71d09b495ded455065202cfdf13803fd69e13b3a Mon Sep 17 00:00:00 2001 From: GearsAD Date: Wed, 2 May 2018 20:31:18 -0500 Subject: [PATCH 20/27] Two handlers completed, three more to go --- examples/marine/rov/highend/synchrony.jl | 16 ++++++++++------ src/cloudgraphs/CloudGraphIntegration.jl | 2 +- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index 17be3b709..5b5906faa 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -78,10 +78,8 @@ function handle_poses!(slam::SyncrSLAM, # set robot parameters in the first pose, this will become a separate node in the future println("[Caesar.jl] Setting robot parameters") - setRobotParameters!(slaml) + setRobotParameters!(slam) end - - sleep(5) end """ @@ -114,8 +112,6 @@ function handle_priors!(slam::SyncrSLAM, fctBody = FactorBody(string(typeof(prior_rpz)), string(typeof(packed_prior_rpz)), "JSON", JSON.json(packed_prior_rpz)) fctRequest = FactorRequest([node_label], fctBody, false, false) @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) - - sleep(5) end # this function handles lcm msgs @@ -131,7 +127,7 @@ end # 0. Constants println("[Caesar.jl] defining constants.") robotId = "HROV" -sessionId = "LCM_13" +sessionId = "LCM_17" # create a SLAM container object slam_client = SyncrSLAM(robotId, sessionId, nothing) @@ -178,8 +174,16 @@ subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) println("[Caesar.jl] Running LCM listener") listener!(lcm_node) +#################################### +########### TESTING ################ +#################################### + + + +#################################### #### TODO stuff +#################################### """ Handle partial x, y, and heading odometry constraints between Pose3 variables. diff --git a/src/cloudgraphs/CloudGraphIntegration.jl b/src/cloudgraphs/CloudGraphIntegration.jl index e41b6b710..b536c2f5e 100644 --- a/src/cloudgraphs/CloudGraphIntegration.jl +++ b/src/cloudgraphs/CloudGraphIntegration.jl @@ -562,7 +562,7 @@ function fullLocalGraphCopy!(fgl::FactorGraph; reqbackendset::Bool=true, reqread conn = fgl.cg.neo4j.connection IDs = getAllExVertexNeoIDs(conn, sessionname=fgl.sessionname, reqbackendset=reqbackendset, reqready=reqready) println("fullLocalGraphCopy: $(length(IDs)) nodes in session $(fgl.sessionname) if reqbackendset=$reqbackendset and reqready=$reqready...") - if length(IDs) > 1 + if length(IDs) > 0 cverts = Dict{Int64, CloudVertex}() unsorted = Int64[] # TODO ensure this is row is sorted From 3f6ee44865999f7b58dd16a14f7777e27887f2e9 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Wed, 2 May 2018 23:00:49 -0500 Subject: [PATCH 21/27] Loop closures now ingesting correctly --- examples/marine/rov/highend/lcmHandlers.jl | 173 ++++++++++++++++++++ examples/marine/rov/highend/synchrony.jl | 180 ++------------------- 2 files changed, 187 insertions(+), 166 deletions(-) create mode 100644 examples/marine/rov/highend/lcmHandlers.jl diff --git a/examples/marine/rov/highend/lcmHandlers.jl b/examples/marine/rov/highend/lcmHandlers.jl new file mode 100644 index 000000000..d86868c39 --- /dev/null +++ b/examples/marine/rov/highend/lcmHandlers.jl @@ -0,0 +1,173 @@ +""" +Adds pose nodes to graph with a prior on Z, pitch, and roll. +""" +function handle_poses!(slam::SyncrSLAM, + msg::pose_node_t) + id = msg.id + println("[Caesar.jl] Received pose msg for x$(id)") + + mean = msg.mean + covar = msg.covar + t = [mean[1], mean[2], mean[3]] + qw = mean[4] + qxyz = [mean[5], mean[6], mean[7]] + q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? + pose = SE3(t,q) + euler = Euler(q) + # + node_label = Symbol("x$(id)") + varRequest = VariableRequest(node_label, "Pose3", nothing, ["POSE"]) + resp = addVariable(slam.syncrconf, slam.robotId, slam.sessionId, varRequest) + # xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call + # slaml.lastposesym = node_label; # update object + # + if id == 0 + println("[Caesar.jl] First pose") + # this is the first msg, and it does not carry odometry, but the prior on the first node. + + # add 6dof prior + initPosePrior = RoME.PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) + packedPrior = convert(RoME.PackedPriorPose3, initPosePrior) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(initPosePrior)), string(typeof(packedPrior)), "JSON", JSON.json(packedPrior)) + fctRequest = FactorRequest([node_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) + + # auto init is coming, this code will likely be removed + # initializeNode!(slaml, node_label) + + # set robot parameters in the first pose, this will become a separate node in the future + println("[Caesar.jl] Setting robot parameters") + setRobotParameters!(slam) + end +end + +""" +Handle ZPR priors on poses. +""" +function handle_priors!(slam::SyncrSLAM, + msg::prior_zpr_t) + + id = msg.id + println("[Caesar.jl] Adding prior on RPZ to x$(id)") + + node_label = Symbol("x$(id)") + # xn = getVert(slam,node_label) + + # 1. Build up the prior + z = msg.z + pitch = msg.pitch + roll = msg.roll + var_z = msg.var_z + var_pitch = msg.var_pitch + var_roll = msg.var_roll + rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) + z_dist = Normal(z, var_z) + prior_rpz = RoME.PartialPriorRollPitchZ(rp_dist, z_dist) + + # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) + packed_prior_rpz = convert(RoME.PackedPartialPriorRollPitchZ, prior_rpz) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + # fctBody = FactorBody(string(typeof(prior_rpz)), string(typeof(packed_prior_rpz)), "JSON", JSON.json(packed_prior_rpz)) + # fctRequest = FactorRequest([node_label], fctBody, false, false) + # @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) +end + +""" +Handle partial x, y, and heading odometry constraints between Pose3 variables. +""" +function handle_partials!(slam::SyncrSLAM, + msg::pose_pose_xyh_t) + println(" --- Handling odometry change...") + return + + origin_id = msg.node_1_id + destination_id = msg.node_2_id + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + println("[Caesar.jl] Adding XYH odometry constraint between(x$(origin_id), x$(destination_id))") + + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw + + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + + origin_label, destination_label + # @show xo = getVert(slam,origin_label) + # @show xd = getVert(slam,destination_label) + + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = PartialPose3XYYaw(xyh_dist) + # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) + packed_xyh_factor = convert(RoME.PackedPartialPose3XYYaw, xyh_factor) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(xyh_factor)), string(typeof(packed_xyh_factor)), "JSON", JSON.json(packed_xyh_factor)) + fctRequest = FactorRequest([origin_label; destination_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) + + # addFactor!(slam, [xo;xd], xyh_factor) + # + # initializeNode!(slam, destination_label) + # println() +end + +""" +Handle loop closure proposals with chance of being a null hypothesis likelihood. +""" +function handle_loops!(slam::SyncrSLAM, + msg::pose_pose_xyh_nh_t) + + println(" --- Handling loop...") + # return nothing + + origin_id = msg.node_1_id + destination_id = msg.node_2_id + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw + + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + confidence = msg.confidence + + # @show xo = getVert(slam.fg,origin_label) + # @show xd = getVert(slam.fg,destination_label) + + # if (destination_id - origin_id == 1) + # warn("Avoiding parallel factor! See: https://github.com/dehann/IncrementalInference.jl/issues/63To ") + # return + # end + + println("[Caesar.jl] Adding XYH-NH loop closure constraint between (x$(origin_id), x$(destination_id))") + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = RoME.PartialPose3XYYawNH(xyh_dist ,[1.0-confidence, confidence]) # change to NH + # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) + packed_xyh_factor = convert(RoME.PackedPartialPose3XYYawNH, xyh_factor) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(xyh_factor)), string(typeof(packed_xyh_factor)), "JSON", JSON.json(packed_xyh_factor)) + fctRequest = FactorRequest([origin_label; destination_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) + + # addFactor!(slaml, [xo;xd], xyh_factor ) + + # NOT USED + # println("[Caesar.jl] Adding P3P3NH loop closure constraint between (x$(origin_id), x$(destination_id))") + + # # line below fails! + # lcf = Pose3Pose3NH( MvNormal(veeEuler(rel_pose), diagm(1.0./covar)), [0.5;0.5]) # define 50/50% hypothesis + # lcf_label = Symbol[origin_label;destination_label] + + # addFactor!(slaml, lcf_label, lcf) +end diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index 5b5906faa..f75375bb8 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -1,6 +1,5 @@ #= LCM Server: an LCM interface to Caesar.jl - =# using Caesar @@ -34,100 +33,22 @@ function setRobotParameters!(sslaml::SyncrSLAM) nothing end - -# TODO -- code below untested - -""" -Adds pose nodes to graph with a prior on Z, pitch, and roll. -""" -function handle_poses!(slam::SyncrSLAM, - msg::pose_node_t) - id = msg.id - println("[Caesar.jl] Received pose msg for x$(id)") - - mean = msg.mean - covar = msg.covar - t = [mean[1], mean[2], mean[3]] - qw = mean[4] - qxyz = [mean[5], mean[6], mean[7]] - q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? - pose = SE3(t,q) - euler = Euler(q) - # - node_label = Symbol("x$(id)") - varRequest = VariableRequest(node_label, "Pose3", nothing, ["POSE"]) - resp = addVariable(slam.syncrconf, slam.robotId, slam.sessionId, varRequest) - # xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call - # slaml.lastposesym = node_label; # update object - # - if id == 0 - println("[Caesar.jl] First pose") - # this is the first msg, and it does not carry odometry, but the prior on the first node. - - # add 6dof prior - initPosePrior = RoME.PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) - packedPrior = convert(RoME.PackedPriorPose3, initPosePrior) - - # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) - fctBody = FactorBody(string(typeof(initPosePrior)), string(typeof(packedPrior)), "JSON", JSON.json(packedPrior)) - fctRequest = FactorRequest([node_label], fctBody, false, false) - @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) - - # auto init is coming, this code will likely be removed - # initializeNode!(slaml, node_label) - - # set robot parameters in the first pose, this will become a separate node in the future - println("[Caesar.jl] Setting robot parameters") - setRobotParameters!(slam) - end -end - -""" -Handle ZPR priors on poses. -""" -function handle_priors!(slam::SyncrSLAM, - msg::prior_zpr_t) - - id = msg.id - println("[Caesar.jl] Adding prior on RPZ to x$(id)") - - node_label = Symbol("x$(id)") - # xn = getVert(slam,node_label) - - # 1. Build up the prior - z = msg.z - pitch = msg.pitch - roll = msg.roll - var_z = msg.var_z - var_pitch = msg.var_pitch - var_roll = msg.var_roll - rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) - z_dist = Normal(z, var_z) - prior_rpz = RoME.PartialPriorRollPitchZ(rp_dist, z_dist) - - # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) - packed_prior_rpz = convert(RoME.PackedPartialPriorRollPitchZ, prior_rpz) - - # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) - fctBody = FactorBody(string(typeof(prior_rpz)), string(typeof(packed_prior_rpz)), "JSON", JSON.json(packed_prior_rpz)) - fctRequest = FactorRequest([node_label], fctBody, false, false) - @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) -end +# Include the LCM handlers... +include(joinpath(dirname(@__FILE__), "lcmHandlers.jl")) # this function handles lcm msgs function listener!(lcm_node::Union{LCMCore.LCM, LCMCore.LCMLog}) # handle traffic - # TODO: handle termination + # TODO: handle termination while true handle(lcm_node) end end - # 0. Constants println("[Caesar.jl] defining constants.") robotId = "HROV" -sessionId = "LCM_17" +sessionId = "LCM_27" # create a SLAM container object slam_client = SyncrSLAM(robotId, sessionId, nothing) @@ -150,10 +71,10 @@ robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) # create new handlers to pass in additional data lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) -# lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) +lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) # lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) -# lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) +lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) # create LCM object and subscribe to messages on the following channels logfile = robotdata("rovlcm_singlesession_01") @@ -163,10 +84,10 @@ lcm_node = LCMLog(logfile) # for direct log file access subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_node_t) # factors -# subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) -subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +subscribe(lcm_node, "*CAESAR_PARTIAL_XYH*", lcm_odom_handler, pose_pose_xyh_t) +# subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) # loop closures come in via p3p3nh factors -# subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) +subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) # sensor data # subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) @@ -178,88 +99,15 @@ listener!(lcm_node) ########### TESTING ################ #################################### - - - +using Unmarshal +packedJson = "{\"vecZij\":[-0.3171846270561218,-7.061707019805908,-0.14232116187023247],\"vecCov\":[0.01,0.0,0.0,0.0,0.01,0.0,0.0,0.0,0.01],\"nullhypothesis\":[0.5,0.5]}" +dictPacked = JSON.parse(packedJson) +packedType = Unmarshal.unmarshal(RoME.PackedPartialPose3XYYawNH, dictPacked) +prior = convert(RoME.PartialPose3XYYawNH, packedType) #################################### #### TODO stuff #################################### -""" -Handle partial x, y, and heading odometry constraints between Pose3 variables. -""" -function handle_partials!(slam::SyncrSLAM, - msg::pose_pose_xyh_t) - - origin_id = msg.node_1_id - destination_id = msg.node_2_id - origin_label = Symbol("x$(origin_id)") - destination_label = Symbol("x$(destination_id)") - - println("[Caesar.jl] Adding XYH odometry constraint between(x$(origin_id), x$(destination_id))") - - delta_x = msg.delta_x - delta_y = msg.delta_y - delta_yaw = msg.delta_yaw - - var_x = msg.var_x - var_y = msg.var_y - var_yaw = msg.var_yaw - - origin_label, destination_label - xo = getVert(slam,origin_label) - xd = getVert(slam,destination_label) - - xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) - xyh_factor = PartialPose3XYYaw(xyh_dist) - addFactor!(slam, [xo;xd], xyh_factor) - - initializeNode!(slam, destination_label) - println() -end - -""" -Handle loop closure proposals with chance of being a null hypothesis likelihood. -""" -function handle_loops!(slaml::SyncrSLAM, - msg::pose_pose_xyh_nh_t) - - origin_id = msg.node_1_id - destination_id = msg.node_2_id - origin_label = Symbol("x$(origin_id)") - destination_label = Symbol("x$(destination_id)") - - delta_x = msg.delta_x - delta_y = msg.delta_y - delta_yaw = msg.delta_yaw - - var_x = msg.var_x - var_y = msg.var_y - var_yaw = msg.var_yaw - confidence = msg.confidence - - xo = getVert(slaml.fg,origin_label) - xd = getVert(slaml.fg,destination_label) - - # if (destination_id - origin_id == 1) - # warn("Avoiding parallel factor! See: https://github.com/dehann/IncrementalInference.jl/issues/63To ") - # return - # end - - println("[Caesar.jl] Adding XYH-NH loop closure constraint between (x$(origin_id), x$(destination_id))") - xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) - xyh_factor = PartialPose3XYYawNH(xyh_dist ,[1.0-confidence, confidence]) # change to NH - addFactor!(slaml, [xo;xd], xyh_factor ) - - # println("[Caesar.jl] Adding P3P3NH loop closure constraint between (x$(origin_id), x$(destination_id))") - - # # line below fails! - # lcf = Pose3Pose3NH( MvNormal(veeEuler(rel_pose), diagm(1.0./covar)), [0.5;0.5]) # define 50/50% hypothesis - # lcf_label = Symbol[origin_label;destination_label] - - # addFactor!(slaml, lcf_label, lcf) -end - # type Cloud # points:: From 113d872568744dc44a6703089b65ab621b1b3315 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 3 May 2018 00:25:35 -0400 Subject: [PATCH 22/27] moving strange folder --- examples/{rov => tobedeleted}/highend/synchrony.jl | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/{rov => tobedeleted}/highend/synchrony.jl (100%) diff --git a/examples/rov/highend/synchrony.jl b/examples/tobedeleted/highend/synchrony.jl similarity index 100% rename from examples/rov/highend/synchrony.jl rename to examples/tobedeleted/highend/synchrony.jl From 681b79625a2fe744e823ded1bf41facbbe6e925d Mon Sep 17 00:00:00 2001 From: GearsAD Date: Thu, 3 May 2018 21:00:12 -0500 Subject: [PATCH 23/27] Working example, just need data --- examples/marine/rov/highend/lcmHandlers.jl | 33 ++++++++++++- examples/marine/rov/highend/synchrony.jl | 48 ++++--------------- .../rov/highend/synchronySDKIntegration.jl | 12 +++++ 3 files changed, 52 insertions(+), 41 deletions(-) diff --git a/examples/marine/rov/highend/lcmHandlers.jl b/examples/marine/rov/highend/lcmHandlers.jl index d86868c39..0f15c5349 100644 --- a/examples/marine/rov/highend/lcmHandlers.jl +++ b/examples/marine/rov/highend/lcmHandlers.jl @@ -79,7 +79,7 @@ end Handle partial x, y, and heading odometry constraints between Pose3 variables. """ function handle_partials!(slam::SyncrSLAM, - msg::pose_pose_xyh_t) + msg::Any) println(" --- Handling odometry change...") return @@ -171,3 +171,34 @@ function handle_loops!(slam::SyncrSLAM, # addFactor!(slaml, lcf_label, lcf) end + +""" +Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client +""" +function handle_clouds!(slaml::SyncrSLAM, + msg::point_cloud_t) + # TODO: interface here should be as simple as slam_client.add_pointcloud(nodeID, pc::SomeCloudType) + + # TODO: check for empty clouds! + + id = msg.id + + last_pose = Symbol("x$(id)") + println("[Caesar.jl] Got cloud $id") + return + + # 2d arrays of points and colors (from LCM data into arrays{arrays}) + points = [[pt[1], pt[2], pt[3]] for pt in msg.points] + colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in msg.colors] + + + # TODO: check if vert exists or not (may happen if msgs are lost or out of order) + vert = getVert(slaml, last_pose, api=IncrementalInference.dlapi) # fetch from database + + # push to mongo (using BSON as a quick fix) + # (for deserialization, see src/DirectorVisService.jl:cachepointclouds!) + serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) + appendvertbigdata!(slaml, vert, "BSONpointcloud", string(serialized_point_cloud).data) + serialized_colors = BSONObject(Dict("colors" => colors)) + appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) +end diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index f75375bb8..2c468d74a 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -2,6 +2,7 @@ LCM Server: an LCM interface to Caesar.jl =# +using Base.Dates using Caesar using Unmarshal using CaesarLCMTypes @@ -41,7 +42,9 @@ function listener!(lcm_node::Union{LCMCore.LCM, LCMCore.LCMLog}) # handle traffic # TODO: handle termination while true - handle(lcm_node) + if !handle(lcm_node) + break + end end end @@ -49,6 +52,7 @@ end println("[Caesar.jl] defining constants.") robotId = "HROV" sessionId = "LCM_27" +sessionId = strip(sessionId) # create a SLAM container object slam_client = SyncrSLAM(robotId, sessionId, nothing) @@ -57,12 +61,6 @@ slam_client = SyncrSLAM(robotId, sessionId, nothing) println("[Caesar.jl] Setting up remote solver") initialize!(slam_client) -# Make sure that this session is not already populated -existingSessions = getSessions(slam_client.syncrconf, sessionId) -if count(session -> session.id == sessionId, existingSessions.sessions) > 0 - error("There is already a session named '$sessionId' for robot '$robotId'. This example will fail if it tries to add duplicate nodes. We strongly recommend providing a new session name.") -end - # Set up the robot setRobotParameters!(slam_client) robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) @@ -73,7 +71,7 @@ robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) -# lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) +lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) # create LCM object and subscribe to messages on the following channels @@ -84,8 +82,8 @@ lcm_node = LCMLog(logfile) # for direct log file access subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_node_t) # factors -subscribe(lcm_node, "*CAESAR_PARTIAL_XYH*", lcm_odom_handler, pose_pose_xyh_t) -# subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler) +subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) # loop closures come in via p3p3nh factors subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) @@ -126,33 +124,3 @@ prior = convert(RoME.PartialPose3XYYawNH, packedType) # serialized_colors = BSONObject(Dict("colors" => cloud.colors)) # appendvertbigdata!(slaml.fg, vert, "BSONcolors", string(serialized_colors).data) # end - -""" -Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client -""" -function handle_clouds!(slaml::SyncrSLAM, - msg::point_cloud_t) - # TODO: interface here should be as simple as slam_client.add_pointcloud(nodeID, pc::SomeCloudType) - - # TODO: check for empty clouds! - - id = msg.id - - last_pose = Symbol("x$(id)") - println("[Caesar.jl] Got cloud $id") - - # 2d arrays of points and colors (from LCM data into arrays{arrays}) - points = [[pt[1], pt[2], pt[3]] for pt in msg.points] - colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in msg.colors] - - - # TODO: check if vert exists or not (may happen if msgs are lost or out of order) - vert = getVert(slaml, last_pose, api=IncrementalInference.dlapi) # fetch from database - - # push to mongo (using BSON as a quick fix) - # (for deserialization, see src/DirectorVisService.jl:cachepointclouds!) - serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) - appendvertbigdata!(slaml, vert, "BSONpointcloud", string(serialized_point_cloud).data) - serialized_colors = BSONObject(Dict("colors" => colors)) - appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) -end diff --git a/examples/marine/rov/highend/synchronySDKIntegration.jl b/examples/marine/rov/highend/synchronySDKIntegration.jl index d55afa15f..8dc3870d4 100644 --- a/examples/marine/rov/highend/synchronySDKIntegration.jl +++ b/examples/marine/rov/highend/synchronySDKIntegration.jl @@ -115,6 +115,18 @@ function initialize!(sslaml::SyncrSLAM; # 2. Confirm that the robot already exists, create if it doesn't. sslaml.robot = syncrRobot(sslaml.syncrconf, sslaml.robotId) + # Make sure that this session is not already populated + if isSessionExisting(slam_client.syncrconf, sslaml.robotId, sslaml.sessionId) + warn("There is already a session named '$sessionId' for robot '$robotId'. This example will fail if it tries to add duplicate nodes. We strongly recommend providing a new session name.") + print(" Should we delete it? [Y/N] - ") + if lowercase(strip(readline(STDIN))) == "y" + deleteSession(sslaml.syncrconf, sslaml.robotId, sslaml.sessionId) + println(" -- Session '$sessionId' deleted!") + else + warn("Okay, but this may break the example! Caveat Userus!") + end + end + # 3. Create or retrieve the session. sslaml.session = syncrSession(sslaml.syncrconf, sslaml.robotId, sslaml.sessionId) From f303a57c3dd759b7ab92044cc449f61b96a3bf55 Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sat, 5 May 2018 10:46:15 -0500 Subject: [PATCH 24/27] Working BigData additions too --- examples/marine/rov/highend/lcmHandlers.jl | 81 ++++++---------------- examples/marine/rov/highend/synchrony.jl | 4 +- 2 files changed, 25 insertions(+), 60 deletions(-) diff --git a/examples/marine/rov/highend/lcmHandlers.jl b/examples/marine/rov/highend/lcmHandlers.jl index 0f15c5349..af4c3bfff 100644 --- a/examples/marine/rov/highend/lcmHandlers.jl +++ b/examples/marine/rov/highend/lcmHandlers.jl @@ -18,8 +18,6 @@ function handle_poses!(slam::SyncrSLAM, node_label = Symbol("x$(id)") varRequest = VariableRequest(node_label, "Pose3", nothing, ["POSE"]) resp = addVariable(slam.syncrconf, slam.robotId, slam.sessionId, varRequest) - # xn = addNode!(slaml, node_label, labels=["POSE"], dims=6) # this is an incremental inference call - # slaml.lastposesym = node_label; # update object # if id == 0 println("[Caesar.jl] First pose") @@ -34,9 +32,6 @@ function handle_poses!(slam::SyncrSLAM, fctRequest = FactorRequest([node_label], fctBody, false, false) @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) - # auto init is coming, this code will likely be removed - # initializeNode!(slaml, node_label) - # set robot parameters in the first pose, this will become a separate node in the future println("[Caesar.jl] Setting robot parameters") setRobotParameters!(slam) @@ -53,8 +48,6 @@ function handle_priors!(slam::SyncrSLAM, println("[Caesar.jl] Adding prior on RPZ to x$(id)") node_label = Symbol("x$(id)") - # xn = getVert(slam,node_label) - # 1. Build up the prior z = msg.z pitch = msg.pitch @@ -70,9 +63,9 @@ function handle_priors!(slam::SyncrSLAM, packed_prior_rpz = convert(RoME.PackedPartialPriorRollPitchZ, prior_rpz) # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) - # fctBody = FactorBody(string(typeof(prior_rpz)), string(typeof(packed_prior_rpz)), "JSON", JSON.json(packed_prior_rpz)) - # fctRequest = FactorRequest([node_label], fctBody, false, false) - # @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) + fctBody = FactorBody(string(typeof(prior_rpz)), string(typeof(packed_prior_rpz)), "JSON", JSON.json(packed_prior_rpz)) + fctRequest = FactorRequest([node_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) end """ @@ -98,10 +91,6 @@ function handle_partials!(slam::SyncrSLAM, var_y = msg.var_y var_yaw = msg.var_yaw - origin_label, destination_label - # @show xo = getVert(slam,origin_label) - # @show xd = getVert(slam,destination_label) - xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) xyh_factor = PartialPose3XYYaw(xyh_dist) # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) @@ -111,11 +100,6 @@ function handle_partials!(slam::SyncrSLAM, fctBody = FactorBody(string(typeof(xyh_factor)), string(typeof(packed_xyh_factor)), "JSON", JSON.json(packed_xyh_factor)) fctRequest = FactorRequest([origin_label; destination_label], fctBody, false, false) @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) - - # addFactor!(slam, [xo;xd], xyh_factor) - # - # initializeNode!(slam, destination_label) - # println() end """ @@ -141,14 +125,6 @@ function handle_loops!(slam::SyncrSLAM, var_yaw = msg.var_yaw confidence = msg.confidence - # @show xo = getVert(slam.fg,origin_label) - # @show xd = getVert(slam.fg,destination_label) - - # if (destination_id - origin_id == 1) - # warn("Avoiding parallel factor! See: https://github.com/dehann/IncrementalInference.jl/issues/63To ") - # return - # end - println("[Caesar.jl] Adding XYH-NH loop closure constraint between (x$(origin_id), x$(destination_id))") xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) xyh_factor = RoME.PartialPose3XYYawNH(xyh_dist ,[1.0-confidence, confidence]) # change to NH @@ -159,46 +135,33 @@ function handle_loops!(slam::SyncrSLAM, fctBody = FactorBody(string(typeof(xyh_factor)), string(typeof(packed_xyh_factor)), "JSON", JSON.json(packed_xyh_factor)) fctRequest = FactorRequest([origin_label; destination_label], fctBody, false, false) @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) - - # addFactor!(slaml, [xo;xd], xyh_factor ) - - # NOT USED - # println("[Caesar.jl] Adding P3P3NH loop closure constraint between (x$(origin_id), x$(destination_id))") - - # # line below fails! - # lcf = Pose3Pose3NH( MvNormal(veeEuler(rel_pose), diagm(1.0./covar)), [0.5;0.5]) # define 50/50% hypothesis - # lcf_label = Symbol[origin_label;destination_label] - - # addFactor!(slaml, lcf_label, lcf) end """ Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client """ -function handle_clouds!(slaml::SyncrSLAM, +function handle_clouds!(slam::SyncrSLAM, msg::point_cloud_t) - # TODO: interface here should be as simple as slam_client.add_pointcloud(nodeID, pc::SomeCloudType) - - # TODO: check for empty clouds! - id = msg.id - - last_pose = Symbol("x$(id)") + last_pose = "x$(id)" println("[Caesar.jl] Got cloud $id") - return # 2d arrays of points and colors (from LCM data into arrays{arrays}) - points = [[pt[1], pt[2], pt[3]] for pt in msg.points] - colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in msg.colors] - - - # TODO: check if vert exists or not (may happen if msgs are lost or out of order) - vert = getVert(slaml, last_pose, api=IncrementalInference.dlapi) # fetch from database - - # push to mongo (using BSON as a quick fix) - # (for deserialization, see src/DirectorVisService.jl:cachepointclouds!) - serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) - appendvertbigdata!(slaml, vert, "BSONpointcloud", string(serialized_point_cloud).data) - serialized_colors = BSONObject(Dict("colors" => colors)) - appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) + # @show size(msg.points) + # @show size(msg.colors) + # points = [[pt[1], pt[2], pt[3]] for pt in msg.points] + points = JSON.json(msg.points) + # @show "HERREEEEEE" + # colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in msg.colors] + colors = JSON.json(msg.colors) + + node = getNode(slam.syncrconf, slam.robotId, slam.sessionId, last_pose) + pointCloud = BigDataElementRequest("PointCloud", "Mongo", "Pointcloud from HAUV", points, "application/binary") + colors = BigDataElementRequest("Colors", "Mongo", "Colors from HAUV", colors, "application/binary") + addDataElement(slam.syncrconf, slam.robotId, slam.sessionId, node.id, pointCloud) + addDataElement(slam.syncrconf, slam.robotId, slam.sessionId, node.id, colors) + # serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) + # appendvertbigdata!(slaml, vert, "BSONpointcloud", string(serialized_point_cloud).data) + # serialized_colors = BSONObject(Dict("colors" => colors)) + # appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) end diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index 2c468d74a..7e2bed640 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -88,7 +88,7 @@ subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) # sensor data -# subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) +subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) println("[Caesar.jl] Running LCM listener") listener!(lcm_node) @@ -102,6 +102,8 @@ packedJson = "{\"vecZij\":[-0.3171846270561218,-7.061707019805908,-0.14232116187 dictPacked = JSON.parse(packedJson) packedType = Unmarshal.unmarshal(RoME.PackedPartialPose3XYYawNH, dictPacked) prior = convert(RoME.PartialPose3XYYawNH, packedType) + +msg_points = [8.13305 -0.666686 -1.98914; 7.33001 -2.7746 -1.95458; 7.41093 -2.6327 -2.01238; 7.69368 -1.95501 -1.92476; 8.2546 -0.762656 -1.90386; 8.12886 -0.662197 -1.97778; 8.2284 -0.664559 -1.92912; 8.30724 -0.669406 -1.89989; 8.25277 -0.572092 -1.90335; 6.95415 -3.5615 -1.93757; 7.00676 -3.48989 -1.9279; 7.11229 -3.3879 -1.89566; 7.24134 -3.10426 -1.88373; 7.14833 -3.04169 -1.94523; 7.22599 -3.04247 -1.92337; 7.42208 -3.01578 -1.86684; 7.41939 -2.91612 -1.89005; 7.3394 -2.77466 -1.91547; 7.39894 -2.73838 -1.89973; 7.49792 -2.7474 -1.85854; 7.53263 -2.67156 -1.87655; 7.58568 -2.63219 -1.88512; 7.68323 -2.31885 -1.89035; 7.7267 -2.31875 -1.85706; 7.69877 -2.09793 -1.89411; 7.73164 -2.133 -1.87529; 7.85301 -2.11651 -1.84519; 7.69317 -2.03746 -1.89559; 7.72678 -2.06448 -1.83644; 7.79448 -1.97703 -1.86775; 7.91017 -1.9528 -1.84304; 8.41928 -0.840704 -1.82909; 8.34555 -0.726141 -1.81307; 8.39264 -0.749951 -1.84908; 8.34362 -0.67159 -1.86608; 8.42085 -0.671137 -1.84409; 8.33107 -0.572257 -1.87484; 8.46793 -0.532441 -1.87504; 8.86929 -0.410264 -1.82218; 7.11603 -3.68444 -1.81304; 7.21039 -3.6857 -1.79351; 7.18567 -3.625 -1.80003; 7.13474 -3.43812 -1.8485; 7.21834 -3.39527 -1.81634; 7.26278 -3.42481 -1.79259; 7.26514 -3.30744 -1.81573; 7.36375 -3.29841 -1.77822; 7.42148 -3.21786 -1.78483; 7.34848 -3.10449 -1.85252; 7.44674 -3.1279 -1.81243; 7.47251 -3.12847 -1.77622; 7.44678 -3.02855 -1.80531; 7.49123 -3.0391 -1.78801; 7.62013 -3.01816 -1.78377; 7.55001 -2.93322 -1.78222; 7.62267 -2.88699 -1.76532; 7.53798 -2.77578 -1.83574; 7.63435 -2.64456 -1.82538; 7.74163 -2.57217 -1.83817; 7.74155 -2.44163 -1.81968; 7.88203 -2.31833 -1.80369; 7.78274 -2.16268 -1.82694; 7.86112 -2.13914 -1.80183; 7.80445 -2.06348 -1.81023; 7.84831 -2.05659 -1.78562; 7.8813 -1.97617 -1.79964; 7.91913 -1.95288 -1.8011; 8.60678 -0.960857 -1.72939; 8.41351 -0.832237 -1.77241; 8.60636 -0.886577 -1.74823; 8.45035 -0.710839 -1.78875; 8.50181 -0.682496 -1.78679; 8.646 -0.660311 -1.75779; 8.70511 -0.668845 -1.73484; 8.97384 -0.411165 -1.74512; 8.71354 -0.29926 -1.73225; 8.79922 -0.081106 -1.79118; 8.93563 -0.0594269 -1.74559; 9.21225 0.23 -1.70039; 7.2188 -3.68633 -1.75465; 7.30254 -3.72104 -1.69494; 7.2783 -3.50739 -1.73681;7.39617 -3.52631 -1.69575; 7.31134 -3.45176 -1.75355; 7.39193 -3.43122 -1.70986; 7.46664 -3.43167 -1.68405; 7.36999 -3.37781 -1.74957; 7.47497 -3.35202 -1.70489; 7.40469 -3.26445 -1.76282; 7.4965 -3.23834 -1.73278; 7.58757 -3.2545 -1.69171; 7.5308 -3.11797 -1.75686; 7.59792 -3.13523 -1.71102; 7.66952 -3.13078 -1.68992; 7.5362 -3.04662 -1.74404; 7.624 -3.04898 -1.7097; 7.70336 -3.02919 -1.69376; 7.66977 -2.98466 -1.72511; 7.69759 -2.95148 -1.69861; 7.78195 -2.96759 -1.69854; 7.65377 -2.82563 -1.74274; 7.73278 -2.85684 -1.72134; 7.81868 -2.85701 -1.67667; 7.74381 -2.75828 -1.71499; 7.82201 -2.75884 -1.6817; 7.77398 -2.67167 -1.75801; 7.95052 -2.65058 -1.68134; 8.13994 -2.05906-1.65793; 8.3829 -1.48036 -1.70232; 8.51415 -1.45724 -1.67472; 8.62934 -1.08777 -1.68123; 8.69963 -1.06701 -1.65084; 8.60998 -0.97559 -1.688; 8.69745 -0.981553 -1.65182; 8.79554 -0.987026 -1.63386; 8.7319 -0.909788 -1.69191; 8.80595 -0.910251 -1.66289; 8.63669 -0.761817 -1.70364; 8.7746 -0.761474 -1.63402; 8.81152 -0.836932 -1.65123; 8.75597 -0.676409 -1.6982; 8.83655 -0.683122 -1.66625; 8.91034 -0.675144 -1.63164; 8.75458 -0.644994 -1.71352; 8.83507 -0.637578 -1.66737; 8.92332 -0.640372 -1.63939; 8.88873 -0.4995 -1.64875; 8.953 -0.472828 -1.65053; 9.21588 -0.407209 -1.70309; 8.83028 -0.300018 -1.68266; 8.93151 -0.13066 -1.62931; 9.11317 -0.0894114 -1.64828; 9.14727 -0.0504241 -1.66238; 9.2853 0.018847 -1.65123; 9.27841 0.100733 -1.63181; 9.38506 0.207492 -1.64225; 7.33163 -3.80425 -1.67524; 7.4505 -3.80611 -1.61566; 7.44236 -3.65921 -1.6304;7.46728 -3.62862 -1.65288; 7.41128 -3.52113 -1.65652; 7.49486 -3.52736 -1.61823; 7.51325 -3.44957 -1.62912; 7.58415 -3.42803 -1.62324; 7.50379 -3.36985 -1.66199; 7.59843 -3.36602 -1.62472; 7.66672 -3.31033 -1.60573; 7.62418 -3.2426 -1.66174; 7.68066 -3.25348 -1.6434; 7.657 -3.10878 -1.66591; 7.7299 -3.137 -1.63033; 7.80446 -3.12522 -1.60822; 7.65915 -3.03175 -1.66533; 7.74211 -3.04956 -1.65322; 7.82173 -3.04205 -1.62122; 7.94453 -3.0225 -1.58794; 7.77205 -2.95296 -1.6639; 7.80651 -2.95254 -1.63594; 7.89167 -2.97837 -1.59225; 7.83161 -2.85462 -1.64224; 7.8735 -2.74977 -1.64852; 7.96272 -2.77469 -1.61417; 8.01345 -2.75962 -1.61409; 7.96332 -2.65158 -1.663; 8.029 -2.67722 -1.6247; 8.13682 -2.64086 -1.59362; 8.1353 -2.57561 -1.59565; 8.23906 -2.57094 -1.58206; 8.28438 -2.1644 -1.59261; 8.18887 -2.10235 -1.64499; 8.24944 -2.08471 -1.60043; 8.33484-2.09048 -1.57878; 8.31898 -2.00616 -1.5851; 8.35917 -1.97498 -1.59124; 8.5274 -1.82453 -1.56482; 8.64767 -1.67922 -1.56243; 8.54714 -1.55463 -1.63745; 8.70146 -1.55133 -1.56993; 8.81851 -1.43037 -1.57818; 8.61639 -1.30276 -1.62393; 8.70436 -1.26571 -1.58612; 8.78891 -1.27392 -1.55777; 8.71293 -1.20614 -1.60376; 8.80067 -1.24169 -1.55108; 8.75034 -1.13227 -1.59358; 8.81222 -1.11705 -1.57819; 8.89147 -1.08098 -1.55968; 8.75996 -0.976659 -1.61642; 8.82927 -0.978143 -1.60558; 8.94628 -0.968476 -1.55149; 8.82372 -0.925277 -1.61951; 8.94282 -0.924494 -1.59144; 9.01365 -0.907619 -1.5636; 8.80925 -0.828699 -1.61766; 8.90396 -0.805497 -1.59344; 8.99768 -0.836899 -1.5561; 8.87327 -0.693405 -1.5989; 8.95383 -0.706185 -1.57566; 9.03817 -0.681261 -1.57155; 8.95894 -0.636554 -1.6068; 9.02782 -0.633858 -1.56722; 9.13775 -0.63882 -1.56831; 8.98859 -0.527506-1.58598; 9.25458 -0.440166 -1.56383; 9.39715 -0.413589 -1.57052; 9.08028 -0.301593 -1.58121; 9.30105 -0.0821714 -1.5894; 9.32646 -0.0825402 -1.54346; 9.31103 0.0188006 -1.60546; 9.36211 -0.0315956 -1.58255; 9.40859 0.0582638 -1.56565; 9.33724 0.164537 -1.60013; 7.41756 -3.88429 -1.58835; 7.49404 -3.88528 -1.56086; 7.59439 -3.80835 -1.54417; 7.58028 -3.76301 -1.56501; 7.6912 -3.75873 -1.52383; 7.63961 -3.64637 -1.56041; 7.68712 -3.66294 -1.52443; 7.70036 -3.55303 -1.50835; 7.60073 -3.47212 -1.57264; 7.73125-3.46561 -1.533; 7.80729 -3.41979 -1.51537; 7.72489 -3.39373 -1.55777; 7.80284 -3.36428 -1.52758; 7.87125 -3.36378 -1.50502; 7.81666 -3.28242 -1.54068; 7.8983 -3.25196 -1.52611; 7.84543 -3.18326 -1.57477; 7.93177 -3.16999 -1.53789; 7.98771 -3.15632 -1.50631; 7.85874 -3.0786 -1.57764; 7.9384 -3.09189 -1.53523; 8.01703 -3.03746 -1.52778; 7.94242 -2.96286 -1.55939; 8.02609 -2.96615 -1.52098; 8.10987 -2.99812 -1.51162; 7.96294 -2.85831 -1.57287; 8.06 -2.86351 -1.52783; 7.98697 -2.81714 -1.54739; 8.05934 -2.77438 -1.55267; 8.14631 -2.76685 -1.51493; 8.05811 -2.70943 -1.57238; 8.15005 -2.67439 -1.54221; 8.28452 -2.661 -1.50804; 8.17109 -2.60351 -1.56; 8.24861 -2.58834 -1.52185; 8.35741 -2.6009 -1.50883; 8.3326 -2.44045 -1.53432; 8.28463 -2.31734 -1.56894; 8.34984 -2.16709 -1.54679; 8.43378 -2.15613 -1.50469; 8.52773 -2.15523 -1.47357; 8.30183 -2.05684 -1.55211; 8.38868 -2.08524 -1.53649; 8.48414 -2.09002 -1.49077; 8.40321 -2.0011 -1.53912; 8.48467 -1.99489 -1.5068; 8.44261 -1.8805 -1.54025; 8.61797 -1.89365 -1.50339; 8.68828 -1.90051 -1.48136; 8.53031 -1.77303 -1.55471; 8.62317 -1.79284 -1.53347; 8.68308 -1.785 -1.49275; 8.67378 -1.71238 -1.50152; 8.7493 -1.67734 -1.4842; 8.70981 -1.57331 -1.51749; 8.78752 -1.51086 -1.50909; 8.75532 -1.37866 -1.53886; 8.81372 -1.41362 -1.49875; 8.97681 -1.44691 -1.46639; 8.82156 -1.32599 -1.50903; 8.92709 -1.31209 -1.47492; 8.84296 -1.24081 -1.50997; 8.90102 -1.20466 -1.4851; 8.8959 -1.13884 -1.52455; 9.03726 -1.13522 -1.47368; 9.07792 -1.11016 -1.47501; 9.03368 -0.993412 -1.51639; 9.12597 -1.02331 -1.47331; 9.05137 -0.9358 -1.5055; 9.14045 -0.912891 -1.48818; 9.19316 -0.911113 -1.47731; 9.05615 -0.835174 -1.52273; 9.11712 -0.836341 -1.47704; 9.21873 -0.833898 -1.44901; 9.03892 -0.718392 -1.51464; 9.1539 -0.706229 -1.49472; 9.20794 -0.693736 -1.45773; 9.1583 -0.647661 -1.52018; 9.26101 -0.653632 -1.47569; 9.33435 -0.639487 -1.4497; 9.40269 -0.663579 -1.44972; 9.07074 -0.555406 -1.51867; 9.18062 -0.490745 -1.5104; 9.26224 -0.505754 -1.47962; 9.29731 -0.474816 -1.51095; 9.40505 -0.414046 -1.52108; 9.61138 -0.415102 -1.45006; 9.71553 -0.411095 -1.47784; 9.25595 -0.302944 -1.48594; 9.2409 -0.130542 -1.48535; 9.4211 -0.0827164 -1.50821; 9.73354 0.164809 -1.45828; 9.8251 0.23 -1.44384; 9.8593 0.207764 -1.45978; 7.62942 -3.88704 -1.50066; 7.70896 -3.88809 -1.45025; 7.72487 -3.78679 -1.47784; 7.75605 -3.76038 -1.46928; 7.86776 -3.76214 -1.42379; 7.71889 -3.69544 -1.47467; 7.80191 -3.65841 -1.45691; 7.74748 -3.56872 -1.48355; 7.8039 -3.55084 -1.4551; 7.86507 -3.56979 -1.41651; 7.8109 -3.47855 -1.46521; 7.91315 -3.45867 -1.43884; 7.92538 -3.39063 -1.45731; 7.99033 -3.38372 -1.42551; 7.954 -3.27693 -1.47893; 8.00046 -3.27787 -1.45075; 8.10351 -3.24747 -1.41526; 8.01241 -3.16879 -1.47875; 8.14747 -3.16122 -1.4294; 8.05673 -3.06469 -1.48182; 8.133 -3.07893 -1.44455; 8.14941 -2.98352 -1.45302; 8.24555 -2.99722 -1.42521; 8.16319 -2.9239 -1.4745; 8.22432 -2.87983 -1.4422; 8.31108 -2.89397 -1.40592; 8.16073 -2.7793 -1.47788; 8.25412 -2.78809 -1.44755; 8.34077 -2.76129 -1.43168; 8.27055 -2.70966 -1.47384; 8.37037 -2.68255 -1.44861; 8.36185 -2.58153 -1.46068; 8.4685 -2.58894 -1.42574; 8.53497 -2.57002 -1.40068; 8.45105 -2.44018 -1.46202; 8.56557 -2.43996 -1.41001; 8.46258 -2.31691-1.46111; 8.55637 -2.31666 -1.42923; 8.63266 -2.31646 -1.39388; 8.58322 -2.16484 -1.4229; 8.49714 -2.05423 -1.44698; 8.59525 -2.08273 -1.43086; 8.65861 -2.11585 -1.40081;8.67609 -2.01421 -1.4132; 8.74035 -2.00187 -1.39731; 8.7028 -1.9114 -1.44236; 8.79662 -1.91038 -1.40894; 8.74193 -1.79138 -1.45802; 8.78986 -1.80376 -1.43111; 8.86592 -1.83737 -1.39425; 8.80818 -1.71824 -1.43973; 8.90449 -1.72161 -1.39564; 8.82366 -1.62996 -1.46048; 8.98643 -1.64979 -1.38163; 9.00804 -1.42194 -1.42528; 9.11259 -1.44395 -1.38835; 8.94866 -1.31938 -1.44049; 9.03378 -1.36176 -1.41113; 9.0982 -1.338 -1.39255; 9.04051 -1.25341 -1.43359; 9.10045 -1.24517 -1.38973; 9.17612 -1.22152 -1.36591; 9.05821 -1.16566 -1.45043; 9.11171 -1.14394 -1.41049; 9.16124 -1.03727 -1.44099; 9.24244 -1.05245 -1.40744; 9.2929 -1.07057 -1.37432; 9.15703 -0.965409 -1.44025; 9.25657 -0.933823 -1.42073; 9.34864 -0.944921 -1.37767; 9.43806 -0.924292 -1.37009; 9.24319 -0.827533 -1.41227; 9.3294 -0.833107 -1.38997; 9.43034 -0.885003 -1.36946; 9.25546 -0.738343 -1.41857; 9.34182 -0.720377 -1.39511; 9.42327 -0.696408 -1.37449; 9.37856 -0.661742 -1.41594; 9.44817 -0.665513 -1.37423; 9.54004 -0.639891 -1.37785; 9.34185 -0.565499 -1.39796; 9.44625 -0.555762 -1.36425; 9.55942 -0.43464 -1.43486; 9.7452 -0.421142 -1.38408; 9.8286 -0.0228573 -1.35198; 9.82844 0.154331 -1.41368; 9.92288 0.0954518 -1.34291;9.90217 0.23 -1.40229; 7.88147 -3.89034 -1.36964; 7.98954 -3.7877 -1.3587; 7.90168 -3.63439 -1.40918; 7.99498 -3.63537 -1.37955; 8.04855 -3.54296 -1.34061; 7.91717 -3.49871 -1.40182; 8.03693 -3.47506 -1.36851; 8.11423 -3.46524 -1.33564; 8.03048 -3.36531 -1.39959; 8.10995 -3.41316 -1.37204; 8.18832 -3.37913 -1.33439; 8.05925 -3.30357 -1.39284; 8.09722 -3.31346 -1.37357; 8.2279 -3.2827 -1.341; 8.3015 -3.28489 -1.31965; 8.24177 -3.19816 -1.34571; 8.29611 -3.23996 -1.31741; 8.35 -3.09338 -1.33829; 8.39175 -3.0973 -1.31292; 8.26362 -2.96658 -1.38953; 8.35598 -3.00774 -1.35367; 8.4177 -3.01966 -1.33592; 8.34203 -2.93658 -1.34356; 8.41807 -2.89459 -1.33133; 8.26445 -2.81937 -1.36981; 8.31499 -2.84199 -1.37649; 8.46734 -2.80265 -1.3194; 8.51493 -2.79455 -1.32239; 8.38928 -2.74193 -1.39083; 8.53433 -2.70854 -1.34475; 8.6111 -2.56986 -1.36511; 8.55654 -2.55126 -1.37422; 8.61519 -2.55108 -1.33598; 8.66242 -2.43975 -1.35405; 8.77324 -2.43958 -1.32211; 8.74682 -2.31617 -1.33942; 8.71132 -2.17388 -1.35847; 8.84283 -2.17269 -1.30538; 8.69707 -2.11977 -1.35836; 8.80198 -2.11206 -1.33891; 8.90996 -2.10415 -1.29518; 8.70858 -2.05141 -1.3385; 8.82067 -2.01068 -1.34296; 8.9311 -2.02181 -1.29465; 8.82262 -1.92134 -1.34516; 8.86836 -1.9435 -1.37192; 8.87987 -1.85412 -1.3607; 9.00311 -1.72521 -1.345; 9.03658 -1.64471 -1.33844; 9.18546 -1.55211 -1.29935; 9.14454 -1.45338 -1.34652; 9.15242 -1.36976 -1.33972; 9.18552 -1.33068 -1.34196; 9.14017 -1.24487 -1.35811; 9.19789 -1.25381 -1.32171; 9.31812 -1.23959 -1.28899; 9.30513 -1.13909 -1.33633; 9.40745 -1.13647 -1.29939; 9.36927 -1.06863 -1.33437; 9.39119 -1.0486 -1.31594; 9.35264 -0.979259 -1.35469; 9.43154 -0.95339 -1.32713; 9.45719 -0.832107 -1.33176; 9.45681 -0.726692 -1.33431; 9.53888 -0.747489 -1.2895; 9.55472 -0.658639 -1.33139; 9.64481 -0.652237 -1.31575; 9.54654 -0.556181 -1.3173; 9.64632 -0.575598 -1.2726; 9.67597 -0.503081 -1.27051; 9.67082 -0.305563 -1.31705; 9.75302 -0.13027 -1.25295; 10.1146 -0.00188613 -1.26858; 9.94753 0.144026 -1.31828; 10.0769 0.122956 -1.26962; 10.0377 0.207972 -1.32521; 10.0907 0.23 -1.32146; 7.99695 -3.81484 -1.32121; 8.10572 -3.783 -1.28751; 8.17481 -3.80469 -1.25325; 8.11985 -3.70831 -1.28876; 8.19635 -3.67064 -1.2556; 8.12309 -3.5842 -1.29837; 8.19825 -3.5838 -1.25423; 8.1474 -3.49073 -1.28894; 8.22659 -3.48138 -1.26825; 8.29591 -3.49193 -1.24322; 8.25398 -3.39082 -1.29193; 8.31934 -3.40568 -1.26631; 8.38905 -3.38715 -1.24853; 8.26628 -3.3146 -1.30824; 8.34161 -3.29156 -1.26788; 8.33949 -3.21519 -1.28758; 8.43685 -3.21624 -1.25472; 8.50179 -3.19176 -1.22664; 8.37567 -3.11798 -1.29999; 8.45543 -3.11923 -1.27612; 8.54946 -3.07889 -1.26931; 8.45996 -3.02107 -1.28303; 8.54519 -3.02266 -1.25773; 8.63731 -3.03535 -1.23309; 8.45677-2.93512 -1.28541; 8.56506 -2.90767 -1.25033; 8.55518 -2.82612 -1.25133; 8.65478 -2.7821 -1.252; 8.59958 -2.74268 -1.28187; 8.65561 -2.72355 -1.26215; 8.73004 -2.72658 -1.23358; 8.68441 -2.63976 -1.26496; 8.83945 -2.64933 -1.2254; 8.86184 -2.56907 -1.20895; 8.84909 -2.43942 -1.28455; 8.87241 -2.31586 -1.26078; 8.99454 -2.31555 -1.20363; 8.82163 -2.15212 -1.28116; 8.90127 -2.17213 -1.26516; 8.95957 -2.11698 -1.25033; 8.89702 -2.02466 -1.27242; 8.98847 -2.04777 -1.23233; 9.07635 -2.00573 -1.22246; 9.04403 -1.94195 -1.25051; 9.11725 -1.96288 -1.21892; 9.18591 -1.69038 -1.26487; 9.25522 -1.62661 -1.2213; 9.35661 -1.61902 -1.18898; 9.35459 -1.54849 -1.22062; 9.37907 -1.49911 -1.22833; 9.41445 -1.43734 -1.25213; 9.50064 -1.4454 -1.19055; 9.45287 -1.35588 -1.2095; 9.51432 -1.34027 -1.1967; 9.44188 -1.23693 -1.21805; 9.51509 -1.21339 -1.20797; 9.43015 -1.14994 -1.23649; 9.53327 -1.12329 -1.21565; 9.5575 -1.05411 -1.21583; 9.64671 -0.942096 -1.25251; 9.7221 -0.939728 -1.20215; 9.66266 -0.825586 -1.18822; 9.74811 -0.893356 -1.20708; 9.76439 -0.758315 -1.17943; 9.80256 -0.665151 -1.24611; 9.86111 -0.665142 -1.20663; 9.9182 -0.648438 -1.18531; 9.67681 -0.55696 -1.25323; 9.88191 -0.547346 -1.16921; 9.86273 -0.493807 -1.19676; 9.96216 -0.451374 -1.19648; 10.0116 -0.417788 -1.24075; 10.1019 -0.416416 -1.23101; 10.1573 -0.415088 -1.21675; 9.8407 -0.306774 -1.23427; 9.96904 -0.307435 -1.19664; 9.86401 -0.130058 -1.21436; 9.9574 -0.13 -1.17264; 10.1522 -0.053565 -1.16365; 10.1282 0.0397982 -1.24595; 10.2048 0.0193841 -1.18396; 10.1985 0.11354 -1.23305; 10.2436 0.23 -1.23262; 8.17166 -3.89417 -1.18918; 8.26415 -3.89536 -1.15539; 8.22316 -3.81827 -1.21798; 8.28595 -3.79644 -1.19171; 8.37809 -3.80543 -1.14508; 8.30134 -3.71474 -1.18533; 8.3827 -3.70659 -1.17136; 8.44784 -3.64089 -1.16989; 8.26316 -3.54506 -1.19109; 8.34649 -3.50134 -1.18692; 8.39575 -3.46969 -1.19697; 8.41082 -3.40649 -1.20352; 8.48437 -3.36932 -1.19141; 8.44782 -3.31114 -1.20817; 8.50316 -3.29661 -1.19529; 8.49683 -3.24139 -1.20189; 8.611 -3.22235 -1.15257; 8.56361 -3.1422 -1.20928; 8.63891 -3.13077 -1.17367; 8.73142 -3.14417 -1.13518; 8.58758 -3.01893 -1.20448; 8.6633 -3.02797 -1.18242; 8.75361 -3.0264 -1.14989; 8.67167 -2.92945 -1.17415; 8.75574 -2.92249 -1.13492; 8.67429 -2.83849 -1.17924; 8.75053 -2.78264 -1.19336; 8.75067 -2.74323 -1.20711; 8.90114 -2.74377 -1.12932; 8.9355 -2.71026 -1.14417; 8.89448 -2.65004 -1.17286; 8.96761 -2.62736 -1.13739; 8.90051 -2.5504 -1.18336; 8.94086 -2.55017 -1.13999; 9.06578 -2.14969 -1.16672; 9.06404 -2.04676 -1.19299; 9.22338 -1.7635 -1.1736; 9.444 -1.75037 -1.1073; 9.33908 -1.67698 -1.17859; 9.43284 -1.64101 -1.14029; 9.47244 -1.61665 -1.10086; 9.45063 -1.55741 -1.15246; 9.53428 -1.53377 -1.11762; 9.52224 -1.4755 -1.14419; 9.61109 -1.45997 -1.11779; 9.55052 -1.35552 -1.14028; 9.6156 -1.37021 -1.11838; 9.69834 -1.41132 -1.09685; 9.55376 -1.28834 -1.15386; 9.62763 -1.23976 -1.13605; 9.61076 -1.16764 -1.15094; 9.72468 -1.12907 -1.0846; 9.64707 -1.06473 -1.1646; 9.74008 -1.0748 -1.13258; 9.82232 -1.03712 -1.10856; 9.77841 -0.943979 -1.15026; 9.81616 -0.947311 -1.12722; 9.78458 -0.843775 -1.16331; 9.85144 -0.886602 -1.1344; 9.95909 -0.887349 -1.09865; 9.8498 -0.779165 -1.12974; 9.92097 -0.750475 -1.09721; 9.88447 -0.672461 -1.15193; 9.95051 -0.66987 -1.13148; 10.0349 -0.665737 -1.091; 9.91181 -0.57641 -1.12112; 9.9741 -0.565906 -1.10512; 10.0322 -0.557936 -1.08372; 9.98071 -0.494436 -1.13086; 10.0717 -0.466237 -1.10687; 10.1262 -0.130059 -1.08455; 10.2103 -0.0839027 -1.10359; 10.295 -0.0638901 -1.07009; 10.2453 0.039848 -1.15115; 10.3153 -0.0124913 -1.09387; 10.4115 0.019487 -1.06954; 10.4129 0.219116 -1.14779; 8.32157 -3.89613 -1.11681; 8.37881 -3.89689 -1.07766; 8.44419 -3.80656 -1.10665; 8.50054 -3.80579 -1.07599; 8.4334 -3.72562 -1.11094; 8.5045 -3.70977 -1.06991; 8.59151 -3.67675 -1.05017; 8.41991 -3.55676 -1.12044; 8.70585 -3.28828 -1.045; 8.66851 -3.22321 -1.11222; 8.75056-3.25548 -1.06756; 8.75835 -3.12319 -1.11044; 8.85612 -3.12477 -1.04741; 8.91929 -3.12479 -1.03573; 8.75757 -3.03328 -1.1131; 8.85732 -3.03673 -1.07033; 8.95723 -3.03543 -1.06226; 8.76357 -2.93038 -1.0959; 8.86112 -2.94811 -1.07233; 8.9378 -2.90039 -1.06428; 8.78361 -2.83451 -1.11314; 8.86281 -2.8389 -1.05817; 8.96642 -2.84558 -1.05303; 8.97096 -2.75428 -1.07534; 9.08488 -2.63939 -1.06917; 9.05244 -2.56843 -1.08573; 9.48168 -1.81375 -1.06256; 9.44902 -1.76041 -1.04792; 9.55354 -1.76228 -1.01998; 9.45634 -1.66418 -1.08521; 9.52515 -1.65549 -1.0481; 9.60344 -1.64321 -1.0246; 9.60205 -1.57375 -1.05106; 9.62972 -1.45287 -1.05376; 9.72043 -1.44075 -1.03361; 9.65117 -1.35867 -1.06208; 9.73841 -1.36487 -1.02511; 9.81332 -1.40898 -1.0037; 9.63962 -1.26313 -1.07301; 9.70885 -1.26152 -1.02821; 9.75727 -1.17843 -1.05505; 9.85652 -1.07044 -1.05251; 9.92729 -1.11395 -0.996764; 9.88122 -0.957964 -1.06081; 9.94558 -0.96789 -1.03345; 10.0101 -0.958689 -1.01416; 9.96055 -0.899567 -1.04588; 10.0335 -0.894606 -1.01414; 9.90412 -0.819615 -1.06992; 9.95568 -0.78675 -1.0397; 10.0615 -0.784521 -0.994391; 9.90072 -0.724492 -1.06989; 10.0103 -0.686742 -1.05015; 10.0705 -0.677452 -1.03337; 10.1414 -0.664146 -0.998512; 9.98994 -0.576773 -1.04985; 10.082 -0.607891 -1.01623; 10.13 -0.558442 -0.986296; 10.0198 -0.505125 -1.0476; 10.0883 -0.503572 -1.02874; 10.2969 -0.420021 -1.05578; 10.382 -0.418505 -1.03596; 10.1984 -0.309595 -1.03337; 10.2216 -0.130372 -1.01329; 10.3305 -0.0854991 -1.04587; 10.3534 -0.0792963 -1.00234; 10.4804 -0.0644978 -0.978096; 10.4003 -0.0344564 -1.03462; 10.4854 0.00486674 -1.00403; 10.4365 0.0878209 -1.02932; 10.5488 0.123682 -0.992736; 10.565 0.0971172 -0.980269; 10.5491 0.197908 -1.01938; 10.6233 0.165857 -0.969248; 8.45337 -3.89786 -1.03967; 8.54736 -3.89911 -0.979667; 8.55353 -3.82369 -1.02655; 8.61725 -3.79894 -0.992154; 8.6174 -3.71186 -0.99995;8.63164 -3.55334 -1.00295; 8.73188 -3.50641 -0.958243; 8.74545 -3.40985 -1.00052; 8.79638 -3.48086 -0.964719; 8.78404 -3.37203 -1.03742; 8.83771 -3.24704 -0.995056; 8.94751 -3.23663 -0.958078; 8.86821 -3.14651 -1.00694; 8.9556 -3.13684 -0.970885; 9.02286 -3.13723 -0.960914; 8.96751 -3.05114 -1.00487; 9.02928 -3.03785 -0.966687; 8.89511 -2.98206 -0.997659; 8.94613 -2.92712 -1.01427; 9.05697 -2.93395 -0.95936; 8.97041 -2.85698 -1.00518; 9.06493 -2.84679 -0.962328; 9.01754 -2.76463 -1.01925; 9.09586 -2.74276 -0.981077; 9.15212 -2.76494 -0.963107; 9.10982 -2.64847 -1.01343; 9.17817 -2.6639 -0.972843; 9.40488 -2.02945 -0.992509; 9.51284 -1.93748 -1.00345; 9.5796 -1.97591 -0.937923; 9.5221 -1.86955 -1.00313; 9.56245 -1.78109 -0.967533; 9.6221 -1.76599 -0.959151; 9.6993 -1.75188 -0.922306; 9.63333 -1.67164 -0.980384; 9.71204 -1.70345 -0.932278; 9.67836 -1.55262 -0.996402; 9.68023 -1.62348 -0.955817; 9.76187 -1.45006 -0.962577; 9.83974 -1.50824 -0.914302; 9.78087 -1.42962 -0.957879; 9.8289 -1.36021 -0.952937; 9.91766 -1.41669 -0.912264; 9.84649 -1.2732 -0.966831; 9.91214 -1.33157 -0.92461; 9.86194 -1.19319 -0.984987; 9.93074 -1.19164 -0.938272; 9.90394 -1.05555 -0.980985; 9.94916 -1.1021 -0.932343; 10.0399 -1.05186 -0.939811; 9.99032 -0.979262 -0.975814; 10.0453 -0.975398 -0.944891; 10.065 -0.91334 -0.95432; 10.1596 -0.897872 -0.919616; 10.0813 -0.797227 -0.940621; 10.1458 -0.788374 -0.915637; 10.0691 -0.73043 -0.971834; 10.1539 -0.692271 -0.943144; 10.2427 -0.668067 -0.918011; 10.1822 -0.615798 -0.940222; 10.257 -0.613929 -0.90503; 10.1875 -0.505976 -0.956471; 10.2924 -0.51048 -0.918316; 10.396 -0.482567 -0.907209; 10.3968 -0.431098 -0.923684; 10.502 -0.418774 -0.949609; 10.3046 -0.310833 -0.934475; 10.3166 -0.130705 -0.9406; 10.401 -0.108662 -0.918108; 10.4856 -0.0822289 -0.901025; 10.4998 0.00467861 -0.94002; 10.603 -0.0249176 -0.89401; 10.6554 0.0795156 -0.952705; 10.6528 0.187375 -0.942993; 10.7716 0.20861 -0.90758; 8.65852 -3.90056 -0.919711; 8.65931 -3.80363 -0.939144; 8.73904 -3.81367 -0.909819; 8.72662 -3.73178 -0.906897; 8.74441 -3.67971 -0.923739; 8.84116 -3.66396 -0.882514; 8.83119 -3.56046 -0.880003; 8.87665 -3.46233 -0.852428; 8.92448 -3.4244 -0.896218; 8.94726 -3.34318 -0.895364; 9.02176 -3.31073 -0.854148; 8.94306 -3.27657 -0.909451; 9.02363 -3.23766 -0.89206; 9.08127 -3.12797 -0.899115; 9.06867 -3.03458 -0.899412; 9.14179 -3.05035 -0.911603; 9.10321 -2.97921 -0.906559; 9.14545 -2.9512 -0.871645; 9.10385 -2.83713 -0.903954; 9.16189 -2.86971 -0.864585; 9.18324 -2.77002 -0.887914; 9.2963 -2.75544 -0.860368; 9.20605 -2.64807 -0.923987; 9.26103 -2.66951 -0.892907; 9.36145 -2.66463 -0.851068; 9.29848 -2.60159 -0.90041; 9.59132 -1.99326 -0.903305; 9.71844 -1.93564 -0.867248; 9.64012 -1.86892 -0.859924; 9.67297 -1.84669 -0.901293; 9.72223 -1.77178 -0.848821; 9.71525 -1.70387 -0.882441; 9.83884 -1.66789 -0.841803; 9.83607 -1.59295 -0.856215; 9.86632 -1.52699 -0.863218; 9.91405 -1.47896 -0.837141; 9.82725 -1.35505 -0.869998; 9.95032 -1.37674 -0.857743; 9.93759 -1.28321 -0.869967; 9.9449 -1.1948 -0.873188; 10.0085 -1.22443 -0.835557; 10.0578 -1.08388 -0.861096; 10.116 -1.09985 -0.849988; 10.0911 -1.02039 -0.873993; 10.1367 -1.00225 -0.851972; 10.2178 -1.01688 -0.829521; 10.1837 -0.927265 -0.861655; 10.2534 -0.903807 -0.836213; 10.1797 -0.811264 -0.862514; 10.2412 -0.794466 -0.827167; 10.0952 -0.726247 -0.87528; 10.2084 -0.70877 -0.864871; 10.278 -0.702075 -0.843668; 10.3402 -0.677216 -0.830273; 10.3042 -0.627884 -0.835242; 10.3263 -0.493717 -0.851925; 10.3715 -0.508342 -0.810803; 10.4559 -0.441632 -0.841572; 10.4427 -0.311854 -0.863554; 10.4995 -0.312497 -0.812518; 10.4868 -0.113362 -0.83724; 10.562 -0.0871804 -0.817783; 10.6167 -0.0458553 -0.823884; 10.8154 0.208784 -0.797257; 8.8226 -3.81541 -0.830979; 8.90019 -3.82991 -0.772084; 8.91869 -3.74701 -0.806164; 8.8851 -3.64767 -0.836407; 8.90953 -3.67112 -0.808496; 8.88186 -3.56714 -0.825452; 8.93591 -3.54554 -0.79489; 8.95768 -3.46156 -0.808402; 8.99491 -3.39319 -0.840133; 9.05349 -3.3365 -0.803314; 9.05558 -3.26883 -0.829553; 9.1153 -3.29161 -0.792029; 9.17282 -3.15061 -0.804655;9.22315 -3.12999 -0.768494; 9.16703 -3.10823 -0.826335; 9.2687 -3.05842 -0.786002; 9.18261 -2.96555 -0.814524; 9.27214 -2.97593 -0.776096; 9.20778 -2.83805 -0.814116; 9.27484 -2.88197 -0.780705; 9.33374 -2.86999 -0.756945; 9.28113 -2.78565 -0.808375; 9.39435 -2.76602 -0.776369; 9.42824 -2.67873 -0.768158; 9.45088 -2.66437 -0.769547; 9.76287 -1.89074 -0.811718; 9.8237 -1.85637 -0.763148; 9.85599 -1.76635 -0.765608; 9.91409 -1.80958 -0.736477; 9.86426 -1.70105 -0.787734; 9.9204 -1.69107 -0.758062; 9.88871 -1.60815 -0.775523; 9.93645 -1.5909 -0.766954; 9.95242 -1.51971 -0.761948; 9.95343 -1.44627 -0.76747; 10.0103 -1.35094 -0.777991; 10.19 -1.09817 -0.801455; 10.2638 -1.09651 -0.752168; 10.2013 -1.04822 -0.790719; 10.2688 -1.03282 -0.758536; 10.3479 -1.00319 -0.735953; 10.2993 -0.917263 -0.763131; 10.3521 -0.913453 -0.730471; 10.2747 -0.825629 -0.761552; 10.3372 -0.826967 -0.732576; 10.2794 -0.741207 -0.764131; 10.3644 -0.718684 -0.7463; 10.4524 -0.684504 -0.742296; 10.4138 -0.621878 -0.747992; 10.4874 -0.654941 -0.731342; 10.4008 -0.523771 -0.761516; 10.4847 -0.501098 -0.720489; 10.4988 -0.442658 -0.733906; 10.4858 -0.313042 -0.756153; 10.5947 -0.313772 -0.707634; 10.5089 -0.132129 -0.735844; 10.8294 0.0661929 -0.738347; 10.8459 0.149717 -0.727051; 10.9071 0.208862 -0.746507; 10.9337 0.222983 -0.703313; 8.88263 -3.90357 -0.747131; 8.9645 -3.86782 -0.71608; 9.0044 -3.83175 -0.698164; 8.95388 -3.77174 -0.748369; 9.018 -3.77455 -0.699861; 8.96524 -3.66655 -0.747572; 9.05587 -3.66778 -0.70463; 9.0362 -3.56199 -0.723803; 9.07057 -3.47531 -0.707329; 9.10666 -3.43301 -0.723133; 9.22036 -3.3586 -0.700726; 9.13606 -3.29191 -0.74447; 9.26453 -3.26214 -0.701581; 9.30301 -3.15677 -0.712248; 9.33438 -3.13183 -0.675994; 9.30329 -3.08301 -0.723214; 9.36537 -3.05575 -0.696422; 9.29901 -2.98205 -0.715471; 9.37051 -2.93705 -0.684343; 9.29275 -2.88735 -0.703457; 9.39549 -2.8568 -0.680111; 9.41128 -2.76594 -0.718066; 9.50192 -2.76636 -0.672918; 9.49342 -2.69056 -0.710823; 9.55748 -2.70987 -0.651009; 9.52123 -2.62825 -0.721525; 9.58702 -2.55715 -0.690026; 9.86504 -1.98592 -0.673386; 9.83904 -1.90104 -0.719018; 9.91467 -1.81298 -0.670913; 9.95498 -1.6995 -0.67985; 9.9998 -1.70629 -0.667584; 9.97282 -1.61201-0.69574; 10.0008 -1.61701 -0.664847; 9.98239 -1.54641 -0.705467; 10.0556 -1.54487 -0.656481; 10.1209 -1.44286 -0.617489; 10.2947 -1.1164 -0.677017; 10.3758 -1.01655 -0.649199; 10.3222 -0.92653 -0.674309; 10.3838 -0.917728 -0.660081; 10.4392 -0.917717 -0.6249; 10.3958 -0.825912 -0.64852; 10.4716 -0.876596 -0.618433; 10.3741 -0.749271 -0.654858; 10.4863 -0.703296 -0.649906; 10.5666 -0.692841 -0.647236; 10.4977 -0.641225 -0.657731; 10.5508 -0.633645 -0.600372; 10.5148 -0.524156 -0.647847; 10.5527 -0.579073 -0.603485; 10.8461 -0.0366059 -0.62071; 10.8965 0.0890991 -0.671638; 10.9111 0.145756 -0.636962; 11.0032 0.23 -0.627441; 9.05212 -3.86213 -0.626384; 9.10368 -3.83389 -0.589752; 9.06018 -3.76764 -0.642721; 9.12407 -3.7716 -0.59039; 9.06214 -3.68591 -0.658071; 9.14087 -3.66355 -0.627186; 9.07268 -3.56595 -0.643912; 9.16869 -3.56671 -0.599734; 9.15869 -3.48356 -0.627338; 9.25066 -3.46457 -0.580898; 9.29063 -3.38358 -0.613651; 9.28096 -3.28903 -0.63359; 9.32463 -3.26972 -0.601616; 9.29968 -3.15337 -0.649159; 9.39529 -3.16846 -0.601045; 9.38991 -3.1119 -0.641869; 9.44648 -3.07255 -0.603698; 9.40183 -2.96843 -0.629518; 9.46983 -2.98976 -0.596485; 9.39433 -2.90595 -0.607982; 9.51461 -2.86178 -0.599598; 9.48489 -2.82916 -0.60647; 9.58222 -2.76669 -0.59228; 9.64774 -2.74595 -0.592537; 9.59634 -2.6838 -0.601971; 9.66874 -2.67814 -0.552376; 9.68725 -2.6276-0.576941; 9.72647 -2.54723 -0.553012; 9.90758 -2.07151 -0.575054; 9.93652 -2.00636 -0.577217; 9.96196 -1.92214 -0.597176; 9.96892 -1.80291 -0.603707; 10.0738 -1.82617 -0.546124; 10.0748 -1.73151 -0.567892; 10.1148 -1.6971 -0.563558; 10.0881 -1.62395 -0.56572; 10.14 -1.53243 -0.557324; 10.2666 -1.40015 -0.553873; 10.2358 -1.34634 -0.521858; 10.3241 -1.33358 -0.522935; 10.3068 -1.23824 -0.540995; 10.328 -1.11674 -0.542055; 10.4274 -1.04352 -0.529785; 10.4623 -1.02143 -0.553902; 10.4175 -0.930838 -0.553472; 10.4655 -0.916603 -0.55359; 10.5632 -0.935223 -0.515087; 10.4343 -0.827223 -0.573108; 10.4815 -0.847226 -0.545281; 10.5471 -0.876398 -0.510695; 10.43 -0.760541 -0.591183; 10.4758 -0.736373 -0.560828; 10.5851 -0.71084 -0.515902; 10.5253 -0.681417 -0.568529; 10.5821 -0.65873 -0.537153; 10.6594 -0.645196 -0.528633; 10.5875 -0.570087 -0.537391; 10.6791 -0.458587 -0.544458; 10.775 -0.425652 -0.533426; 10.8981 -0.423825 -0.552032; 10.8411 -0.0978789 -0.571274; 10.8411 -0.0888615 -0.582055; 10.9249 -0.0264893 -0.546548; 10.9261 0.0192926 -0.545198; 11.017 0.0579211 -0.523436; 11.0327 0.114633 -0.509409; 11.0247 0.219561 -0.546751; 9.13401 -3.88286 -0.542171; 9.23566 -3.83606 -0.512983; 9.16565 -3.7727 -0.543084; 9.22428 -3.79056 -0.486523; 9.1724 -3.70597 -0.543871; 9.22707 -3.66529 -0.532484; 9.18839 -3.56721 -0.551451; 9.23591 -3.55989 -0.514824; 9.18098 -3.51176 -0.555283; 9.25142 -3.4944 -0.51626; 9.33877 -3.45711 -0.521174; 9.36724 -3.4005 -0.520207; 9.37316 -3.2872 -0.524639; 9.43731 -3.27961 -0.494252; 9.41737 -3.23522 -0.486149; 9.44297 -3.18611 -0.511249; 9.42923 -3.13417 -0.530244; 9.54554 -3.08516 -0.501576; 9.47091 -3.00829 -0.551036; 9.57903 -2.95621 -0.529331; 9.54007 -2.8522 -0.533218; 9.60488 -2.88915 -0.501066; 9.61311 -2.80917 -0.511008; 9.66146 -2.77383 -0.514546; 9.72326 -2.67082 -0.530536; 9.69106 -2.59987 -0.519396; 9.78343 -2.6097 -0.496791; 9.85339 -2.54673 -0.45387; 9.96194 -2.08105 -0.526526; 9.98992 -2.10501 -0.473729; 9.96532 -2.01189 -0.507028; 10.0741 -1.98847 -0.461177; 10.0403 -1.92805 -0.501399; 10.0763 -1.84426 -0.479384; 10.1206 -1.80831 -0.481322; 10.095 -1.67432 -0.518906; 10.1296 -1.72665 -0.481059; 10.1748 -1.62399 -0.483968; 10.2036 -1.54187 -0.502278; 10.2703 -1.49952 -0.486091; 10.2839 -1.43934 -0.516859; 10.3567 -1.43306 -0.45851; 10.2731 -1.34561 -0.469004; 10.3504 -1.29875 -0.464616; 10.3105 -1.21846 -0.475187; 10.3641 -1.2166 -0.449772; 10.389 -1.12929 -0.467602; 10.4842 -1.09972 -0.453199; 10.5035 -1.0298 -0.46109; 10.5095 -0.925923 -0.449358; 10.562 -0.913069 -0.474072; 10.5235-0.875823 -0.493299; 10.5755 -0.847254 -0.444991; 10.6486 -0.874719 -0.442086; 10.5152 -0.745579 -0.474602; 10.6098 -0.72881 -0.441183; 10.668 -0.704612 -0.472231; 10.6269 -0.67736 -0.462538; 10.6845 -0.656031 -0.441673; 10.6837 -0.549441 -0.44047; 10.7252 -0.47774 -0.45695; 10.832 -0.436446 -0.417683; 10.9006 -0.42435 -0.494748; 10.9906 -0.425173 -0.440588; 10.817 -0.316434 -0.493608; 10.8399 -0.133414 -0.473096; 10.9478 -0.0790336 -0.440996; 11.0012 -0.0271278 -0.405769; 11.0734 0.0451085 -0.418459; 11.1224 0.142695 -0.442441; 11.134 0.219649 -0.464518; 11.2241 0.21969 -0.409383; 9.15957 -3.90736 -0.469634; 9.25841 -3.85499 -0.429155; 9.32101 -3.83809 -0.39955; 9.27863 -3.79779 -0.420027; 9.33436 -3.76469 -0.383557; 9.27622 -3.69747 -0.452286; 9.34542 -3.65553 -0.419123; 9.2856 -3.56843 -0.456056; 9.3708 -3.56123 -0.41958; 9.41806 -3.58524 -0.387678; 9.37675 -3.49567 -0.442354; 9.43548 -3.4844 -0.396513; 9.41001 -3.40734 -0.467251; 9.45519 -3.40614 -0.417845; 9.45388 -3.29184 -0.424363; 9.47167 -3.23612 -0.437456; 9.56918 -3.18272 -0.421374; 9.59219 -3.10925 -0.429349; 9.67667 -3.08344 -0.403573; 9.59387 -2.96852 -0.434853; 9.66496 -3.02152 -0.416208; 9.62622 -2.89677 -0.400081; 9.69332 -2.89152 -0.366005; 9.6992 -2.80242 -0.394381; 9.77776 -2.76767 -0.362737; 9.85018 -2.63657 -0.440492; 9.97502 -2.43629 -0.366159; 9.96797 -2.31336 -0.415605; 10.0396 -2.31319 -0.364445; 10.0502 -2.12313 -0.395372; 10.0894 -2.08957 -0.373141; 10.0658 -2.02509 -0.37354; 10.0918 -2.0323 -0.348432; 10.0759 -1.95349 -0.423612; 10.1568 -1.93258 -0.389476; 10.1579 -1.83671 -0.394966; 10.2208 -1.82539 -0.363142; 10.163 -1.71146 -0.411641; 10.2315 -1.73675 -0.360608; 10.2614 -1.62783 -0.380429; 10.3083 -1.4989 -0.405604; 10.3309 -1.5393 -0.370478; 10.4198 -1.51594 -0.353024; 10.3875 -1.43364 -0.372789; 10.4397 -1.40652 -0.373154; 10.3851 -1.32754 -0.387951; 10.4347 -1.34206 -0.364522; 10.4101 -1.22638 -0.374471; 10.4665 -1.23764 -0.342884; 10.5187 -1.10276 -0.381078; 10.5569 -1.12557 -0.343408; 10.5215 -0.999399 -0.412178; 10.564 -1.03515 -0.360002; 10.5259 -0.916759 -0.405869; 10.5916 -0.919947 -0.367008; 10.6131 -0.857335 -0.366437; 10.6539 -0.851079 -0.362332; 10.6271 -0.746216 -0.355702; 10.7258 -0.703061 -0.364764; 10.7349 -0.658854 -0.360367; 10.7315 -0.55736 -0.347733; 10.7763 -0.544265 -0.352559; 10.7479 -0.489967 -0.356104; 10.8291 -0.475586 -0.357747; 11.0468 -0.42641 -0.326376; 10.8922 -0.318025 -0.353456; 10.9554 -0.099999 -0.346942; 11.0013 -0.0583439 -0.389831; 11.126 -0.0273701 -0.351725; 11.0735 0.0192046 -0.37782; 11.1455 0.0192098 -0.321653; 11.2073 0.0402121 -0.340988; 11.0773 0.094468 -0.383908; 11.1634 0.129231 -0.328862; 11.2223 0.141244 -0.342217; 11.1902 0.209478 -0.354315; 11.2094 0.20957 -0.296154; 9.30444 -3.83842 -0.34937; 9.32073 -3.90952 -0.349854; 9.36666 -3.81484 -0.320111; 9.37005 -3.69958 -0.354802; 9.41774 -3.65713 -0.320255; 9.40024 -3.56384 -0.336953; 9.4463 -3.57622 -0.331685; 9.47226 -3.49578 -0.331204; 9.535 -3.47073 -0.291814; 9.49175 -3.40837 -0.368557; 9.56566 -3.40926 -0.318625; 9.49773 -3.31171 -0.348818; 9.59434 -3.27983 -0.320825; 9.64734 -3.28817 -0.300255; 9.60318 -3.1984 -0.348311; 9.68166 -3.20004 -0.298423; 9.6893 -3.09489 -0.32597; 9.776 -3.12598 -0.299906; 9.70061 -3.00429 -0.329133; 9.77493 -3.04837 -0.309153; 9.71977 -2.91212 -0.29993; 9.75535 -2.91006 -0.313941; 9.68724 -2.8425 -0.333163; 9.7858 -2.8159 -0.290334; 9.86666 -2.76807 -0.312336; 9.83238 -2.73378 -0.326506; 9.86917 -2.6896 -0.313544; 9.9292 -2.63016 -0.300365; 9.94467 -2.54601 -0.324965; 9.99699 -2.56401 -0.278673; 10.0824 -2.43595 -0.261875; 10.0588 -2.31321 -0.284891; 10.0795 -2.16064 -0.341524; 10.1274 -2.12275 -0.291957; 10.0929 -2.03216 -0.295061; 10.1359 -2.0684 -0.290021; 10.2053 -1.99769 -0.284405; 10.1139 -1.95331 -0.317806; 10.2205 -1.93892 -0.277415; 10.1951 -1.83674 -0.315092; 10.2356 -1.84007 -0.26516; 10.2504 -1.74033 -0.294437; 10.3213 -1.73914 -0.267677; 10.2956 -1.67087 -0.282066; 10.3457 -1.62068 -0.282245; 10.3855 -1.53825 -0.289448; 10.4214 -1.51614 -0.270344; 10.3859 -1.47809 -0.271088; 10.4733 -1.42922 -0.2635; 10.4102 -1.34291 -0.28123; 10.454 -1.33114 -0.30695; 10.4205 -1.28575 -0.273282; 10.5618 -1.21945 -0.283685; 10.5855 -1.1564 -0.281123; 10.583 -1.04575 -0.28233; 10.6477 -1.01788 -0.23278; 10.615 -0.951652 -0.275288; 10.6816 -0.924112 -0.266073; 10.6355 -0.856011 -0.297032; 10.7016 -0.856814 -0.253401; 10.7616 -0.903999 -0.226513; 10.7053 -0.732814 -0.26972; 10.7473 -0.663042 -0.29337; 10.8108 -0.669855 -0.250981; 10.8048 -0.547728 -0.253146; 10.8311 -0.476259 -0.281828; 10.9266 -0.456382 -0.243354; 11.0306 -0.426875 -0.267966; 10.9647 -0.319041 -0.269109; 11.0184 -0.319736 -0.212315; 10.9874 -0.135382 -0.24851; 11.0481 -0.0715804 -0.244176; 11.1979 -0.0276254 -0.295102; 11.1303 0.039961 -0.251273; 11.1996 0.0191852 -0.264362; 11.162 0.110479 -0.21951; 11.2058 0.12348 -0.25323; 11.1934 0.23 -0.28611; 9.3308 -3.90977 -0.250184; 9.38165 -3.81792-0.245416; 9.45995 -3.80769 -0.21086; 9.38964 -3.72947 -0.279179; 9.47933 -3.68575 -0.235764; 9.50964 -3.56801 -0.243825; 9.46994 -3.51543 -0.264041; 9.59309 -3.48991 -0.226894; 9.52824 -3.41873 -0.266959; 9.61154 -3.40862 -0.224342; 9.62392 -3.29423 -0.243159; 9.66449 -3.30101 -0.219465; 9.69367 -3.23911 -0.220725; 9.73016 -3.11929 -0.252037; 9.79572 -3.12838 -0.20255; 9.79417 -3.03543 -0.220278; 9.72693 -2.95897 -0.250664; 9.7922 -2.92347 -0.227478; 9.81952 -2.82852 -0.216248; 9.86686 -2.789 -0.220222; 9.91103 -2.73562 -0.214347; 9.95115 -2.64561 -0.212745; 10.013 -2.63203 -0.192996; 10.0379 -2.55448 -0.209835; 10.0829 -2.43583 -0.208495; 10.1125 -2.31312 -0.205419; 10.1318 -2.15251 -0.209517; 10.2325 -2.13659 -0.158218; 10.2439 -2.0355 -0.190613; 10.2566 -1.95202 -0.158138; 10.3451 -1.93142 -0.148513; 10.2846 -1.83621 -0.208129; 10.3557 -1.82483 -0.17482; 10.3044 -1.73965 -0.212922; 10.3714 -1.73447 -0.185736; 10.3793 -1.64943 -0.179669; 10.4383 -1.62153 -0.196454; 10.4396 -1.55007 -0.172384; 10.4788 -1.46292 -0.15339; 10.5389 -1.42463 -0.18919; 10.5042 -1.35936 -0.182164; 10.5793 -1.31794 -0.166783; 10.5775 -1.24264 -0.186301; 10.612 -1.14501 -0.18449; 10.6331 -1.05633 -0.178497; 10.6742 -1.0389 -0.173252; 10.6316 -0.99706 -0.190606; 10.7302 -0.928719 -0.16027; 10.7694 -0.926909 -0.159048; 10.7291 -0.855626 -0.165113; 10.7701 -0.883856 -0.146772; 10.7389 -0.751469 -0.179787; 10.7782 -0.743712 -0.144529; 10.8251 -0.671326 -0.171348; 10.8963 -0.662618 -0.129921; 10.858 -0.562518 -0.194905; 10.9002 -0.557792 -0.154199; 10.8613 -0.503177 -0.166753; 10.9294 -0.474295 -0.156344; 11.0316 -0.428774 -0.182046; 11.1202 -0.428244 -0.15285; 11.0192 -0.320327 -0.15443; 11.0327 -0.136206 -0.162612; 11.134 -0.0728184 -0.134712; 11.1829 -0.0282952 -0.148128; 11.2018 0.0329562 -0.15871; 11.1882 0.0998269 -0.155042; 11.2066 0.111841 -0.173878; 11.2126 0.219879 -0.173946; 9.44529 -3.91132 -0.149979; 9.49427 -3.80934 -0.161857; 9.53218 -3.80678 -0.141232; 9.50514 -3.71404 -0.175733; 9.54075 -3.69662 -0.139521; 9.52606 -3.588 -0.135668; 9.59216 -3.597 -0.148517; 9.60725 -3.51988 -0.148004; 9.64134 -3.4725 -0.139183; 9.63431 -3.41995 -0.165428; 9.70485 -3.4053 -0.119701; 9.72142 -3.30674 -0.111939; 9.73026 -3.25172 -0.122651; 9.77992 -3.21616 -0.119816; 9.83293 -3.12628 -0.143176; 9.89459 -3.12218 -0.095807; 9.83158 -3.00493 -0.145281; 9.90262 -3.04102 -0.119394; 9.81341 -2.91813 -0.147709; 9.88295 -2.92742 -0.114411; 9.81194 -2.85515 -0.127436; 9.93258 -2.82036 -0.111794; 9.93841 -2.7682 -0.15533; 9.99637 -2.74241 -0.113612; 10.1157 -2.70891 -0.0896455; 10.0325 -2.59818 -0.160119; 10.1123 -2.6464 -0.105417; 10.1219 -2.55081 -0.0978052; 10.1271 -2.4356 -0.128344; 10.2094 -2.31291 -0.125088; 10.2188 -2.14781 -0.116154; 10.2691 -2.06102 -0.0963642; 10.3287 -2.04065 -0.0965721; 10.2912 -1.96907 -0.1229; 10.3677 -1.956 -0.0767181; 10.3662 -1.83308 -0.0925492; 10.3752 -1.75013-0.0945388; 10.4632 -1.72669 -0.0874461; 10.456 -1.63509 -0.104181; 10.4962 -1.53961 -0.0996119; 10.5018 -1.47597 -0.078597; 10.5747 -1.42086 -0.0950892; 10.5831 -1.35219-0.0725823; 10.6022 -1.24453 -0.0683365; 10.651 -1.23134 -0.1054; 10.635 -1.19089 -0.0959604; 10.6853 -1.13479 -0.0674529; 10.6378 -1.09211 -0.100932; 10.7118 -1.02986 -0.0786482; 10.766 -1.06309 -0.0380907; 10.738 -0.965153 -0.0957822; 10.8 -0.927778 -0.0688831; 10.751 -0.907326 -0.100805; 10.8081 -0.876406 -0.0643759; 10.8235 -0.753191 -0.0619738; 10.8495 -0.673867 -0.0906926; 10.9259 -0.668588 -0.0639289; 10.866 -0.581508 -0.105038; 10.9311 -0.55934 -0.0478941; 10.9528 -0.506196 -0.0782152; 11.0237 -0.457769 -0.0468696; 11.1558 -0.429165 -0.0652461; 11.0548 -0.321289 -0.0676317; 11.0423 -0.137271 -0.0759134; 11.178 -0.0709777 -0.0655774; 11.1934 0.0187883 -0.0588069; 11.2262 0.12638 -0.0332021; 11.2304 0.202697 -0.0447574; 9.48841 -3.91198 -0.0484101; 9.4807 -3.8433 -0.0481969; 9.5873 -3.81078 -0.0605782; 9.60544 -3.71672 -0.0473572; 9.59668 -3.57965 -0.0759092; 9.67834 -3.62126 -0.0316276; 9.62915 -3.50785 -0.0693338; 9.6949 -3.50902 -0.0384363; 9.71442 -3.43416 -0.0600424; 9.78346 -3.4023 -0.0300033; 9.71604 -3.3172 -0.0765045; 9.79721 -3.3057 -0.0270053; 9.81187 -3.26424 -0.0223129; 9.8761 -3.21914 -0.0301949; 9.9068 -3.12813 -0.0245095; 9.92994 -3.04014 -0.0403276; 9.97864 -3.03018 0.00583927; 9.9157 -2.93955 -0.024115; 9.92056 -2.86067 -0.0260255; 9.9984 -2.78976 -0.0355203; 10.0432 -2.76855 -0.0222038; 10.1149 -2.7276 -0.0297619; 10.1388 -2.62621 -0.0173701; 10.1388 -2.55025 -0.00815439; 10.2232 -2.43526 -0.0200658; 10.2531 -2.31285 -0.0436093; 10.3052 -2.31275 0.0113838; 10.3237 -2.14667 -0.00688503; 10.3454 -2.04674 -0.0136621; 10.4118 -2.0059 0.00715203; 10.3783 -1.97404 -0.00128607; 10.4501 -1.93079 -0.0108286; 10.346 -1.85441 -0.0306203; 10.4427 -1.83799 0.0181193; 10.4556 -1.73382 0.0015301; 10.4941 -1.64648 -0.00620359; 10.5084 -1.60691 0.02967; 10.5653 -1.54766 0.00370405; 10.5766 -1.44651 -0.000262131; 10.597 -1.31796 -0.0268298; 10.6923 -1.37427 0.0262318; 10.6156 -1.25734 0.0053355; 10.6844 -1.31604 0.0301884; 10.7556 -1.2539 0.0632488; 10.6341 -1.21275 0.0187258; 10.7053 -1.1606 0.0152856; 10.7249 -1.1006 0.009931; 10.774 -1.06858 0.0462306; 10.8107 -0.96275 0.0217242; 10.8409 -0.888021 0.0127966; 10.9055 -0.897708 0.0329045; 10.973 -0.902488 0.0602761; 10.857 -0.755483 0.00726615; 10.9371 -0.774282 0.0349608; 10.9618 -0.662474 0.0116281; 11.0089 -0.692082 0.032674; 10.9012 -0.582051 0.00945054; 10.9902 -0.56359 0.0483869; 10.9653-0.499247 0.0229407; 11.0517 -0.482807 0.057561; 11.1556 -0.429956 0.0226927; 11.0899 -0.322254 0.0196285; 11.1421 -0.322966 0.0784549; 11.19 -0.0870306 0.00676893; 11.2061 -0.098813 0.0513141; 11.2254 0.111134 0.0145849; 9.51349 -3.91239 0.0282982; 9.65285 -3.91419 0.0828884; 9.60679 -3.81277 0.0252326; 9.64593 -3.82429 0.0450638; 9.66954-3.71873 0.056857; 9.68726 -3.61448 0.0433194; 9.76677 -3.66415 0.0918766; 9.69802 -3.54462 0.0468132; 9.7778 -3.51429 0.0816422; 9.8091 -3.4251 0.0539615; 9.81904 -3.32762 0.0587491; 9.88576 -3.30903 0.0674805; 9.84083 -3.26827 0.0618625; 9.89304 -3.23162 0.0623171; 9.96025 -3.15558 0.0984303; 9.96784 -3.14473 0.0539837; 9.95104 -3.02581 0.0779228; 9.99709 -3.06066 0.0720247; 9.92063 -2.94471 0.0492693; 9.98636 -2.9397 0.0893263; 9.9512 -2.85651 0.0311476; 10.0091 -2.8451 0.0670005; 10.1471 -2.74937 0.0863036; 10.1513 -2.65379 0.0640796; 10.2038 -2.6337 0.0899845; 10.1878 -2.59667 0.109622; 10.2244 -2.56193 0.070482; 10.2963 -2.54381 0.0773766; 10.3182 -2.43492 0.0901331; 10.3302 -2.31275 0.093862; 10.3678 -2.14225 0.0891059; 10.3908 -2.04601 0.0875503; 10.4586 -2.0845 0.115836; 10.3941 -1.98543 0.0777159; 10.4473 -1.95897 0.104954; 10.4671 -1.85374 0.108656; 10.487 -1.7611 0.107101; 10.5408 -1.72598 0.0798703; 10.5246 -1.65581 0.082447; 10.5587 -1.65522 0.138912; 10.5954 -1.5778 0.114642; 10.6138 -1.48699 0.127996; 10.6976 -1.45839 0.109139; 10.706 -1.36312 0.100601; 10.768 -1.37315 0.146504; 10.7672 -1.28407 0.118452; 10.7742 -1.17606 0.102722; 10.7586 -1.10074 0.123954; 10.783 -1.0378 0.110819; 10.823 -1.00743 0.122387; 10.9285 -0.989704 0.153037; 10.8165 -0.916561 0.0978409; 10.9234 -0.901174 0.142611; 10.9794 -0.880056 0.105747; 10.9571 -0.771813 0.136678; 10.9893 -0.784238 0.118435; 10.9755 -0.705777 0.105339; 11.023 -0.671918 0.130651; 10.9703 -0.582615 0.125216; 11.0558 -0.554695 0.126333; 11.0477 -0.500206 0.124951; 11.1532 -0.451544 0.13252; 11.1899 -0.432096 0.094811; 11.1765 -0.323643 0.137548; 11.1819 -0.139031 0.128713; 9.66803 -3.91448 0.160904; 9.68167 -3.83275 0.139195; 9.71935 -3.73914 0.146122; 9.75329 -3.74039 0.186591; 9.73009 -3.66445 0.14291; 9.79516 -3.61631 0.167757; 9.81156 -3.53439 0.165696; 9.85973 -3.53504 0.174083; 9.88514 -3.43661 0.16436; 9.91968 -3.33778 0.15187; 9.9324 -3.26364 0.171076; 9.96587 -3.26228 0.148313; 10.0064 -3.15503 0.16343; 10.0269 -3.04411 0.158226; 10.122 -3.05528 0.196648; 10.0333 -2.937 0.183527; 9.99945 -2.87763 0.156472; 10.1361 -2.85202 0.18725; 10.1614 -2.77304 0.189261; 10.1994 -2.72079 0.149458; 10.2306 -2.64484 0.17535; 10.2607 -2.56801 0.182002; 10.3104 -2.56147 0.154165; 10.3777 -2.43467 0.173807; 10.3896 -2.31265 0.177618; 10.3972 -2.14935 0.191564; 10.4218 -2.13394 0.171837; 10.4573 -2.06454 0.19972; 10.4973 -1.97092 0.197013; 10.5502 -1.93074 0.241106; 10.5263 -1.86399 0.186685; 10.5521 -1.85337 0.222047; 10.5292 -1.77798 0.177711; 10.5679 -1.75734 0.219026; 10.5983 -1.67236 0.21163; 10.6198 -1.59904 0.193113; 10.6632 -1.5628 0.22872; 10.6831 -1.47237 0.203924; 10.7422 -1.36268 0.21491; 10.7929 -1.36314 0.209731; 10.7433-1.285 0.203277; 10.8224 -1.27723 0.234101; 10.7542 -1.21055 0.190429; 10.8028 -1.18923 0.221532; 10.8163 -1.07892 0.213447; 10.8218 -0.993087 0.208063; 10.9079 -0.990788 0.268045; 10.9489 -0.917558 0.202053; 11.0063 -0.889393 0.208372; 11.0199 -0.792608 0.226216; 11.0435 -0.692776 0.228854; 11.1049 -0.648901 0.226356; 11.0502 -0.582899 0.228836; 11.0551 -0.508939 0.231282; 11.1186 -0.495549 0.217472; 11.1351 -0.433019 0.211238; 11.1754 -0.324239 0.196255; 9.71779 -3.9152 0.240655; 9.7209 -3.82929 0.219661;9.78537 -3.83818 0.262427; 9.73293 -3.77838 0.232228; 9.81154 -3.74175 0.267178; 9.8335 -3.59283 0.259291; 9.84913 -3.6665 0.252067; 9.83307 -3.54771 0.253246; 9.90276 -3.51235 0.250016; 9.91563 -3.45316 0.258771; 9.99503 -3.40504 0.259323; 9.93484 -3.33578 0.233572; 9.99505 -3.3316 0.274067; 9.95062 -3.26329 0.236381; 10.0159 -3.2629 0.284236; 10.0294 -3.1597 0.245116; 10.0868 -3.13792 0.295504; 10.0456 -3.03477 0.22686; 10.1144 -3.06073 0.268022; 10.0199 -2.96398 0.225421; 10.1615 -2.95756 0.279155; 10.1498 -2.85099 0.268252; 10.1892 -2.85781 0.288387; 10.2081 -2.76092 0.269119; 10.3165 -2.74657 0.307328; 10.2511 -2.65509 0.252428; 10.3256 -2.65762 0.287955; 10.3313 -2.56044 0.280142; 10.3759 -2.43454 0.228726; 10.4441 -2.43435 0.286411; 10.4396 -2.31258 0.262067; 10.4762 -2.14849 0.28635; 10.5027 -2.03824 0.282851; 10.5412 -2.0832 0.285123; 10.5054 -1.97983 0.291078; 10.5474 -1.95489 0.295666; 10.5529 -1.85034 0.289534; 10.5918 -1.76733 0.311378; 10.6131 -1.7146 0.322472; 10.6616 -1.66564 0.28751; 10.6654 -1.58331 0.285353; 10.7183 -1.49173 0.298007; 10.7451 -1.41171 0.267141; 10.7864 -1.39221 0.314237; 10.8057 -1.30131 0.316984; 10.8379 -1.21368 0.315087; 10.8438 -1.11419 0.321609; 10.9106 -1.08875 0.359816; 10.9057 -1.02822 0.304154; 10.924 -0.915753 0.291507; 11.0332 -0.897894 0.312195; 11.0282 -0.786576 0.322606; 11.0661 -0.690616 0.318515; 11.1156 -0.680957 0.356167; 11.0644 -0.592654 0.322735; 11.1034 -0.638662 0.363944; 11.0682 -0.497561 0.287967; 11.1369 -0.502114 0.341839; 11.1878 -0.326066 0.372982; 9.73302 -3.91546 0.293436; 9.78308 -3.91615 0.348235; 9.80943 -3.8368 0.352877; 9.85973 -3.83664 0.35808; 9.80248 -3.7047 0.299118; 9.86931 -3.74361 0.349098; 9.90687 -3.66781 0.334066; 9.91818 -3.54688 0.346471; 9.94704 -3.46704 0.373741; 10.0028 -3.434 0.346304; 10.005 -3.34213 0.352791; 10.0382 -3.27585 0.356124; 10.0828 -3.25537 0.376386; 10.1032 -3.16611 0.347883; 10.1795 -3.1294 0.40056; 10.128 -3.05941 0.357694; 10.1405 -2.97174 0.361335; 10.1922 -2.91637 0.35571; 10.1809 -2.85829 0.347943; 10.2016 -2.8695 0.397045; 10.1883 -2.79087 0.372264; 10.2761 -2.74601 0.414852; 10.2965 -2.75781 0.348615; 10.3559 -2.65977 0.366561; 10.3546 -2.59531 0.360912; 10.4143 -2.54802 0.38176; 10.4317 -2.43416 0.368865; 10.4538 -2.31261 0.345755; 10.5214 -2.31247 0.404601; 10.4862 -2.13286 0.340183; 10.5416 -2.14768 0.400602; 10.5053 -2.04426 0.353585; 10.5492 -2.07604 0.379814; 10.5231 -2.01491 0.381613; 10.5849 -1.97031 0.386895; 10.6127 -1.87566 0.402688; 10.6437 -1.84272 0.372576; 10.6353 -1.77178 0.400931; 10.6544 -1.7481 0.384542; 10.6928 -1.67313 0.401282; 10.7137 -1.59719 0.394169; 10.7374 -1.50487 0.380037; 10.7933 -1.50374 0.439698; 10.7493 -1.42209 0.377772; 10.8257 -1.41082 0.443243; 10.8635 -1.39027 0.397191; 10.818 -1.31861 0.396416; 10.8836 -1.3338 0.429602; 10.834 -1.20946 0.417157; 10.8718 -1.19812 0.364883; 10.8721 -1.11014 0.41144; 10.8883 -1.12984 0.41824; 10.9401 -1.03093 0.413989; 11.0638 -0.909606 0.436634; 11.0976 -0.880623 0.45409; 11.0761 -0.799109 0.422237; 11.0689 -0.697764 0.393634; 11.1156 -0.696144 0.435628; 11.1165 -0.619945 0.422897; 11.1322 -0.522911 0.430493; 11.1801 -0.435214 0.447526; 11.1855 -0.32666 0.431745; 9.79616 -3.91641 0.427769; 9.83199 -3.85181 0.430348; 9.86996 -3.84123 0.456604; 9.84542 -3.76361 0.450479; 9.87025 -3.73509 0.416817; 9.93606 -3.66921 0.441923; 9.93061 -3.58253 0.443103; 9.98175 -3.54441 0.447138; 9.94631 -3.46176 0.417899; 10.0182 -3.45999 0.455494; 10.0488 -3.34919 0.454558; 10.0362 -3.28206 0.43299; 10.1205 -3.27288 0.468152; 10.1441 -3.15635 0.456655; 10.1887 -3.19187 0.47578; 10.1635 -3.06194 0.433491; 10.1965 -3.05996 0.472011; 10.1773 -2.97744 0.468458; 10.215 -2.92769 0.473557; 10.2267 -2.85882 0.456631; 10.2805 -2.7914 0.458916; 10.3347 -2.75775 0.46032; 10.3826 -2.65415 0.450427; 10.4328 -2.66692 0.493102; 10.4039 -2.56814 0.456083; 10.4364 -2.5915 0.473339; 10.4873 -2.43384 0.482873; 10.501 -2.31258 0.459333; 10.5328 -2.31254 0.517041; 10.5159 -2.13226 0.452947; 10.5575 -2.15513 0.486451; 10.5311 -2.06222 0.511668; 10.5677 -2.09203 0.474586; 10.5917 -1.9884 0.492449; 10.657 -2.00313 0.517884; 10.6378 -1.8644 0.471493; 10.6973 -1.89178 0.505402; 10.688 -1.77747 0.493111; 10.7185 -1.69226 0.496471; 10.7212 -1.5971 0.508268; 10.7914 -1.59564 0.512104; 10.7991 -1.50533 0.510979; 10.8395 -1.41573 0.513727; 10.8535 -1.32411 0.503119; 10.8844 -1.30535 0.520137; 10.866 -1.22878 0.469009; 10.9137 -1.20395 0.514802; 10.9379 -1.10924 0.504909; 10.9594 -1.03543 0.490937; 11.0738 -0.986877 0.507865; 11.0782 -0.912256 0.51058; 11.1028 -0.890798 0.5047; 11.078 -0.802291 0.526132; 11.1043 -0.815446 0.518198; 11.1262 -0.708642 0.524045; 11.1498 -0.627262 0.530239; 11.1568 -0.522553 0.564914; 9.8857 -3.91767 0.539336; 9.918 -3.83516 0.528951; 9.92025 -3.7561 0.523226; 9.96135 -3.74716 0.567217; 9.98682 -3.6838 0.559923; 9.99553 -3.56604 0.54869; 10.0376 -3.47644 0.539717; 10.0993 -3.42678 0.588635; 10.0681 -3.40676 0.531337; 10.1144 -3.35938 0.550062; 10.1289 -3.27175 0.54031; 10.1866 -3.27168 0.596725; 10.1488 -3.18566 0.553019; 10.2122 -3.15215 0.55368; 10.2531 -3.04081 0.572746; 10.2541 -2.97384 0.576415; 10.2726 -2.85921 0.537873; 10.335 -2.79172 0.572114; 10.4064 -2.74614 0.587985; 10.4339 -2.67127 0.5593; 10.442 -2.57871 0.573303; 10.5173 -2.43362 0.568364; 10.5644 -2.3125 0.5751; 10.5787 -2.1548 0.571851; 10.6264 -2.0912 0.571866; 10.6549 -2.07088 0.630401; 10.635 -2.0134 0.584222; 10.6667 -1.97101 0.606481; 10.6955 -1.87047 0.580143; 10.7138 -1.77762 0.579697; 10.7538 -1.80683 0.633011; 10.7238 -1.72466 0.565654; 10.7772 -1.70365 0.615156; 10.7526 -1.64048 0.549217; 10.7739 -1.61074 0.600507; 10.8069 -1.51061 0.599542; 10.8697 -1.46935 0.555134; 10.8527 -1.41098 0.589231; 10.8796 -1.40991 0.588782; 10.8566 -1.27777 0.637785; 10.8828 -1.32373 0.605759; 10.9301 -1.20799 0.595378; 10.9663 -1.10956 0.622638; 11.0137 -1.02718 0.619212; 11.0524 -0.902952 0.594047; 11.0982 -0.922013 0.577529; 11.1195 -0.810183 0.610089; 11.1232 -0.7299 0.604163; 11.1441 -0.612432 0.628357; 11.1607 -0.509843 0.592501; 9.9302 -3.91836 0.649204; 9.92634 -3.84303 0.62133; 9.98603 -3.8277 0.66511; 9.95522 -3.7482 0.647049; 9.99856 -3.78754 0.645254; 9.99754 -3.69118 0.631726; 10.0312 -3.57309 0.653733; 10.0507 -3.49701 0.641707; 10.1165 -3.45075 0.652888; 10.1396 -3.36932 0.639894; 10.1712 -3.29571 0.642438; 10.1908 -3.27393 0.640753; 10.2395 -3.17417 0.685222; 10.3369 -3.13377 0.684834; 10.2813 -3.07195 0.647207; 10.3315 -3.07211 0.678705; 10.2708 -2.95921 0.637451; 10.3098 -2.99913 0.677141; 10.3189 -2.87603 0.661379; 10.3835 -2.76911 0.647031; 10.4332 -2.74592 0.701103; 10.4435 -2.67062 0.671229; 10.4771 -2.56847 0.674374; 10.5553 -2.43338 0.655072; 10.5761 -2.31254 0.660306; 10.5885 -2.15451 0.685175; 10.5978 -2.10621 0.681661; 10.7125 -2.05986 0.693246; 10.6832 -1.98434 0.674546; 10.7126 -1.89776 0.673409; 10.7413 -1.81072 0.678643; 10.7414 -1.74223 0.69706; 10.8047 -1.68758 0.691801; 10.8089 -1.61006 0.688935; 10.8507 -1.52304 0.695302; 10.8799 -1.48266 0.710313; 10.9066 -1.41008 0.707778; 10.8529 -1.33526 0.658506; 10.9034 -1.31452 0.701629; 11.0237 -1.31012 0.734844; 10.9792 -1.18703 0.659261; 11.0277 -1.19631 0.722101; 11.0307 -1.10857 0.714533; 11.0372 -1.03476 0.70967; 11.1065 -0.878562 0.757947; 11.1445 -0.706425 0.744244; 11.1525 -0.585691 0.774703; 9.9672 -3.91893 0.732675; 10.0058 -3.84319 0.740288; 10.0534 -3.76995 0.733382; 10.0565 -3.6927 0.71748; 10.0673 -3.52553 0.697399; 10.1021 -3.56827 0.759493; 10.1208 -3.47282 0.746045; 10.1651 -3.40175 0.727922; 10.2225 -3.36075 0.782113; 10.2338 -3.28893 0.750194; 10.263 -3.18184 0.751411; 10.3289 -3.15529 0.754992; 10.3373 -3.09225 0.756034; 10.292 -2.95065 0.71407; 10.3483 -2.99303 0.764737; 10.354 -2.87542 0.765045; 10.403 -2.76885 0.734865; 10.4325 -2.77702 0.763897; 10.4754 -2.69222 0.775131; 10.4829 -2.60886 0.764006; 10.5584 -2.53958 0.783934; 10.5903 -2.43307 0.770399; 10.5871 -2.31259 0.745653; 10.67 -2.3124 0.809279; 10.6608 -2.15374 0.775676; 10.6023 -2.13062 0.739177; 10.6691 -2.08267 0.787851; 10.698 -2.00646 0.782607; 10.7764 -1.98185 0.827634; 10.7361 -1.91498 0.785233; 10.7427 -1.82011 0.775389; 10.7718 -1.84432 0.835852; 10.7587 -1.74874 0.817471; 10.8176 -1.70921 0.786165; 10.8335 -1.60738 0.792264; 10.9104 -1.58761 0.83094; 10.8996 -1.50232 0.792256; 10.9251 -1.41501 0.808763; 10.9392 -1.32053 0.807244; 11.0104-1.29253 0.807802; 10.9794 -1.20804 0.812284; 11.0362 -1.1868 0.83908; 10.9852 -1.13088 0.830795; 11.0394 -1.10931 0.829009; 11.1162 -0.878449 0.774739; 11.1261 -0.802965 0.800647; 11.1328 -0.74462 0.827849; 11.1464 -0.661763 0.793066; 11.1599 -0.531597 0.821363; 10.0166 -3.86337 0.84095; 10.0547 -3.77111 0.814749; 10.0669 -3.71342 0.797971; 10.1099 -3.68801 0.840645; 10.0623 -3.57998 0.798873; 10.1372 -3.57975 0.843299; 10.1603 -3.50266 0.837278; 10.1994 -3.47838 0.834328; 10.2008 -3.39952 0.839468; 10.2608 -3.28984 0.846316; 10.3034 -3.27479 0.881468; 10.3163 -3.17646 0.848083; 10.3406 -3.08311 0.849949; 10.3421 -2.98572 0.864247; 10.3929 -2.92013 0.864731; 10.431 -2.87606 0.866688; 10.3967 -2.83945 0.869768; 10.4421 -2.76904 0.85698; 10.4861 -2.6783 0.859959; 10.5168 -2.59295 0.853585; 10.5439 -2.60558 0.900373; 10.6035 -2.53906 0.872657; 10.6126 -2.43272 0.913822; 10.6704 -2.43279 0.86249; 10.6421 -2.31259 0.920493; 10.6649 -2.31247 0.86575; 10.6735 -2.15353 0.833611; 10.7037 -2.08964 0.861557; 10.7346 -2.00207 0.8889; 10.7809 -1.92328 0.887315; 10.7971 -1.82017 0.894766; 10.8335 -1.71769 0.887207; 10.8668 -1.6385 0.902884; 10.8995 -1.60835 0.879078; 10.9273 -1.50194 0.881827; 10.9477 -1.41627 0.891535; 10.9401 -1.33392 0.897203; 11.0181 -1.29275 0.925815; 11.0138 -1.20764 0.893208; 11.0267 -1.12053 0.91918; 11.1144 -0.832105 0.877437; 11.1265 -0.758472 0.880246; 10.042 -3.89054 0.914752; 10.0753 -3.85049 0.936824; 10.1142 -3.78549 0.931887; 10.1405 -3.69604 0.943452; 10.145 -3.58404 0.938033; 10.1736 -3.48655 0.94455; 10.1924 -3.51335 0.919352; 10.2239 -3.40964 0.943935; 10.2515 -3.30437 0.94097; 10.3097 -3.28283 0.930268; 10.3408 -3.17266 0.944845; 10.3659 -3.09442 0.94197; 10.3921 -2.98001 0.954957; 10.4263 -2.9714 0.96367; 10.3704 -2.88838 0.936427; 10.4384 -2.86252 0.984644; 10.4087 -2.83971 0.926681; 10.4807 -2.79869 0.974427; 10.5006 -2.7049 0.942127; 10.5222 -2.63843 0.923034; 10.5551 -2.60445 0.961686; 10.5986 -2.54439 0.974408; 10.6594 -2.43253 0.975347; 10.654 -2.31261 0.978609; 10.7187 -2.14091 0.964473; 10.7853 -2.01087 0.969032; 10.8132 -1.91883 0.965095; 10.817 -1.85525 1.00594; 10.8834 -1.82559 0.975404; 10.8305 -1.74853 0.996682; 10.893 -1.72647 0.984144; 10.8753-1.59987 1.01525; 10.9112 -1.62452 0.979989; 10.913 -1.51897 0.998739; 10.9551 -1.43303 0.990137; 10.0732 -3.92067 1.06867; 10.1287 -3.87295 1.02103; 10.1353 -3.80501 1.03535; 10.1556 -3.70405 1.04516; 10.168 -3.59199 1.04699; 10.2038 -3.58522 1.04534; 10.1701 -3.50712 1.01756; 10.2216 -3.47989 1.05194; 10.2921 -3.30722 1.01744; 10.3239 -3.27599 1.02243; 10.3521 -3.18548 1.03716; 10.3812 -3.10989 1.04331; 10.4348 -3.071 1.07526; 10.4078 -2.98998 1.04811; 10.449 -2.9843 1.07351; 10.4136 -2.92089 1.00631; 10.4674 -2.88307 1.05684; 10.5047 -2.81087 1.07263; 10.5585 -2.70454 1.06126; 10.6002 -2.62053 1.06287; 10.6177 -2.54379 1.07118; 10.7171 -2.4322 1.096; 10.7061 -2.31254 1.07004; 10.73 -2.12826 1.06572; 10.7792 -2.07866 1.09705; 10.7636 -2.02063 1.12004; 10.8073 -1.99726 1.07876; 10.8299 -1.93981 1.07902; 10.8358 -1.83999 1.08695; 10.8688 -1.73306 1.08921; 10.8945 -1.74781 1.06127; 10.9222 -1.6189 1.09717; 10.9557 -1.52847 1.09502; 11.0094 -1.38989 1.04789; 11.0357 -1.33233 1.12172; 10.1329 -3.88165 1.11293; 10.1853 -3.86292 1.1658; 10.1638 -3.81261 1.13034; 10.171 -3.71869 1.13769; 10.1999 -3.67852 1.11704; 10.1953 -3.60276 1.17935; 10.2074 -3.58248 1.12571; 10.2452 -3.50027 1.13238; 10.2653 -3.4307 1.13302; 10.3069 -3.40131 1.16991; 10.2826 -3.33385 1.1713; 10.3442 -3.30612 1.15459; 10.3809 -3.18495 1.13218; 10.4291 -3.1705 1.14238; 10.4168 -3.06517 1.15401; 10.4447 -3.09525 1.15161; 10.4346 -3.00311 1.16293; 10.4492 -2.9164 1.14477; 10.4887 -2.84095 1.16013; 10.5665 -2.78126 1.16017; 10.5804 -2.71454 1.1381; 10.6115 -2.61931 1.16604; 10.6328 -2.55521 1.15508; 10.6784 -2.53728 1.16568; 10.7279 -2.31258 1.18745; 10.7258 -2.15244 1.18239; 10.7997 -2.11111 1.17746; 10.8106 -2.01987 1.18695; 10.8198 -1.94312 1.18215; 10.862 -1.83757 1.16962; 10.9021 -1.82703 1.21044; 10.8469 -1.74923 1.1723; 10.9259 -1.7197 1.19531; 10.9359 -1.63353 1.18184; 10.9493 -1.52875 1.15293; 11.0315 -1.50666 1.21279; 11.0158 -1.42905 1.14788; 11.0329 -1.33265 1.20014; 10.1605 -3.92195 1.21715; 10.2129 -3.8638 1.22488; 10.181 -3.75804 1.18766; 10.1982 -3.81146 1.21809; 10.2559 -3.70043 1.23365; 10.2702 -3.58389 1.23728; 10.2824 -3.51185 1.23476; 10.3007 -3.43116 1.21301; 10.3339 -3.41981 1.25092; 10.3693 -3.30127 1.2459; 10.4007 -3.2008 1.21455; 10.4383 -3.20097 1.24936; 10.4614 -3.10612 1.23857; 10.4261 -3.01878 1.24726; 10.4635 -2.97333 1.24928; 10.5138 -2.90988 1.25985; 10.5119 -2.84888 1.25704; 10.6006 -2.79329 1.27172; 10.623 -2.73347 1.25236; 10.6247 -2.64747 1.26377; 10.6563 -2.62094 1.25333; 10.6825 -2.5456 1.2642; 10.7386 -2.4319 1.21365;10.7662 -2.43173 1.27495; 10.7434 -2.31262 1.27595; 10.7937 -2.1272 1.27527; 10.8231 -2.01578 1.26762; 10.8378 -1.94884 1.27741; 10.8701 -1.8664 1.23714; 10.8972 -1.84606 1.31292; 10.9272 -1.73612 1.2735; 10.9606 -1.62802 1.25198; 10.9969 -1.63624 1.32683; 11.0016 -1.58592 1.30911; 10.1841 -3.92235 1.30348; 10.2049 -3.879 1.31389; 10.235 -3.81004 1.32502; 10.271 -3.70858 1.33758; 10.2862 -3.58494 1.35128; 10.3025 -3.60433 1.30513; 10.3181 -3.50649 1.33892; 10.3019 -3.45204 1.30181; 10.3716 -3.41922 1.33514;10.3959 -3.30704 1.32635; 10.4239 -3.27489 1.34961; 10.4756 -3.19259 1.32083; 10.4898 -3.12481 1.34343; 10.5 -3.00348 1.33494; 10.5513 -3.01151 1.37991; 10.5111 -2.92316 1.35699; 10.5829 -2.84113 1.35967; 10.6163 -2.74632 1.35209; 10.631 -2.64907 1.3464; 10.6647 -2.59832 1.33332; 10.6929 -2.54197 1.3673; 10.7239 -2.43164 1.3272; 10.7865 -2.1272 1.33081; 10.9046 -2.0317 1.39332; 10.8858 -1.98103 1.41992; 10.9325 -1.83295 1.38678; 10.9732 -1.73591 1.39714; 11.0033 -1.71113 1.3342; 10.9685 -1.6605 1.42542; 11.0089 -1.64786 1.37963; 11.0168 -1.55164 1.39638; 10.2236 -3.8773 1.42129; 10.2597 -3.83352 1.43068; 10.3086 -3.71287 1.42153; 10.3033 -3.59594 1.42644; 10.3291 -3.59063 1.45926; 10.3044 -3.5222 1.40305; 10.3468 -3.50426 1.44452; 10.3837 -3.40797 1.44755; 10.3991 -3.3136 1.41491; 10.4538 -3.28758 1.45147; 10.4688 -3.2179 1.44169; 10.5051 -3.11697 1.45069; 10.54 -3.0826 1.4221; 10.5226 -3.02109 1.4434; 10.5703 -3.06885 1.45898; 10.5084 -2.93943 1.41187; 10.5639 -2.92558 1.46736; 10.5812 -2.82599 1.44221; 10.6991 -2.62524 1.4669; 10.7564 -2.55314 1.48899; 10.7801 -2.53546 1.46838; 10.8273 -2.15104 1.486; 10.8841 -2.12577 1.51986; 10.8702 -2.03351 1.47435; 10.8935 -2.01817 1.48592; 10.895 -1.98097 1.47994; 10.9532 -1.83342 1.4928; 10.9677 -1.73311 1.49748; 11.0051 -1.71147 1.45284; 10.9944 -1.63262 1.48414; 11.0188 -1.59714 1.44341; 11.0056 -1.04188 1.55165; 10.2506 -3.89524 1.50938; 10.2719 -3.839 1.52119; 10.2897 -3.73748 1.50555; 10.3439 -3.71096 1.53534; 10.3443 -3.58672 1.52906; 10.3603 -3.51126 1.52094; 10.4193 -3.53364 1.57252; 10.3934 -3.44702 1.52999; 10.4318 -3.41369 1.53375; 10.4178 -3.33596 1.58513; 10.4481 -3.31094 1.53628; 10.4763 -3.2316 1.53238; 10.5098 -3.12416 1.54095; 10.541 -3.08119 1.5294; 10.5516 -3.03768 1.55693; 10.5717 -2.9302 1.56167; 10.6043 -2.84195 1.55485; 10.6297 -2.74414 1.57455; 10.6838 -2.73886 1.53111; 10.7028 -2.63685 1.56462; 10.7251 -2.56303 1.57815; 10.7716 -2.53508 1.52546; 10.8387 -2.43109 1.51816; 10.8377 -2.31259 1.55141; 10.8405 -2.1425 1.57462; 10.8451 -2.04769 1.5544; 10.8429 -1.98157 1.58953; 10.9038 -1.98091 1.54011; 10.9237 -1.84859 1.56654; 10.9649 -1.73202 1.58369; 10.9794 -1.65257 1.59026; 10.9957 -1.55529 1.6046; 10.997 -1.04256 1.60983; 11.0222 -0.851524 1.61365; 10.2583 -3.92368 1.62287; 10.2914 -3.84587 1.60181; 10.3387 -3.83208 1.64823; 10.3475 -3.73405 1.6323; 10.3821 -3.59391 1.6336; 10.3872 -3.51939 1.64246; 10.412 -3.43404 1.65288; 10.4454 -3.41437 1.63658; 10.4794 -3.312 1.63879; 10.4905 -3.25091 1.62669; 10.5446 -3.1866 1.62896; 10.5326 -3.16714 1.65511; 10.5622 -3.10521 1.60463; 10.5736 -3.0525 1.65245; 10.5726 -2.94434 1.64473; 10.6087 -2.84228 1.65943; 10.6324 -2.73957 1.66626; 10.6669 -2.73558 1.61846; 10.6445 -2.64705 1.66578; 10.6756 -2.6241 1.64379; 10.668 -2.55163 1.70768; 10.7168 -2.55183 1.66363; 10.8329 -2.31269 1.63878; 10.8489 -2.12572 1.63205; 10.8584 -2.03778 1.63985; 10.8819 -1.8488 1.66334; 10.9058 -1.73724 1.63951; 10.9632 -1.74561 1.69458; 10.9817 -1.65264 1.68973; 10.9855 -1.59217 1.66948; 10.9913 -1.21341 1.6628; 10.3296 -3.85009 1.72005; 10.3757 -3.71696 1.71613; 10.3761 -3.60224 1.73782; 10.41 -3.52186 1.73552; 10.4381 -3.51126 1.74268; 10.4166 -3.43479 1.75046; 10.4483 -3.41475 1.72303; 10.4677 -3.31442 1.73566; 10.5031 -3.24943 1.73434; 10.5418 -3.20659 1.69156; 10.5212 -3.14397 1.74927; 10.5384 -3.06727 1.76819; 10.5694 -3.04653 1.73747; 10.5778 -2.95331 1.74357; 10.5938 -2.84491 1.74821; 10.6187 -2.73659 1.76521; 10.6474 -2.63489 1.75433; 10.6816 -2.61376 1.71757; 10.6582 -2.55125 1.76417; 10.7037 -2.53341 1.77714; 10.7346 -2.31313 1.79829; 10.7801 -1.9561 1.80466; 10.8411 -1.84193 1.75202; 10.8596 -1.65107 1.83025; 10.8655 -1.55107 1.81893; 10.9665 -1.59262 1.78555; 10.909 -1.4147 1.79048; 10.9235 -1.33729 1.83635; 10.947 -1.14134 1.82789; 10.9628 -0.909601 1.8127; 10.9412 -0.897216 1.85794; 10.9784 -0.832982 1.79575; 10.3422 -3.84371 1.83159; 10.3594 -3.73637 1.80554; 10.3907 -3.59666 1.82286; 10.4033 -3.5488 1.81554; 10.4349 -3.51202 1.85693; 10.4179 -3.43507 1.80805; 10.4501 -3.41512 1.84424; 10.4309 -3.3368 1.81634; 10.4728 -3.31519 1.84988; 10.4941 -3.25734 1.83477; 10.5059 -3.18202 1.83345; 10.5557 -3.14875 1.84477; 10.5345 -3.07175 1.84836; 10.5647 -3.01438 1.84275; 10.5488 -2.95921 1.84898; 10.5835 -2.92599 1.88663; 10.5565 -2.87683 1.86587; 10.5826 -2.84767 1.82566; 10.5803 -2.78083 1.85119; 10.6501 -2.54168 1.86719; 10.6846 -2.43031 1.90207; 10.6793 -2.3134 1.90533;10.707 -2.31327 1.85193; 10.6922 -2.15144 1.87312; 10.7129 -2.12603 1.90199; 10.7334 -2.03861 1.89733; 10.7602 -1.96296 1.89234; 10.7879 -1.85278 1.89776; 10.8039 -1.74107 1.9013; 10.8432 -1.72732 1.85339; 10.8052 -1.65216 1.90917; 10.8386 -1.57066 1.90389; 10.882 -1.33007 1.92665; 10.9267 -1.25809 1.93582; 10.9311 -1.1458 1.91876; 10.9261-1.07565 1.94829; 10.9772 -0.793091 1.88763; 10.3338 -3.92496 1.89261; 10.3503 -3.84874 1.8793; 10.3859 -3.77 1.89776; 10.4089 -3.64974 1.93035; 10.403 -3.54627 1.94311; 10.4249 -3.44083 1.93027; 10.4518 -3.41559 1.91774; 10.4579 -3.31905 1.91564; 10.4711 -3.27067 1.92498; 10.487 -3.16975 1.90654; 10.5332 -3.03746 1.9268; 10.5386 -2.95395 1.93924; 10.5612 -2.86609 1.9508; 10.6427 -2.54112 1.95346; 10.6738 -2.43019 1.95857; 10.6857 -2.31344 1.96518; 10.7022 -2.14278 1.96147; 10.7045 -2.06689 1.98893; 10.7454-2.03751 1.97976; 10.7453 -1.96014 1.99149; 10.7648 -1.84738 1.99142; 10.7903 -1.74154 1.99646; 10.8021 -1.6523 1.99732; 10.8748 -1.339 2.00611; 10.8969 -1.25185 2.03039;10.9087 -1.16239 2.03619; 10.9154 -1.07647 2.00602; 10.9338 -0.919646 2.03306; 10.9718 -0.751987 2.05016; 10.9761 -0.731596 2.03416; 10.9785 -0.657114 2.08164; 10.3742 -3.87372 2.04559; 10.4223 -3.69017 2.03065; 10.4061 -3.56976 2.02612; 10.436 -3.4542 2.01583; 10.4892 -3.28339 2.00282; 10.5379 -2.96045 2.04332; 10.596 -2.92659 2.00575; 10.5581 -2.87745 2.04062; 10.63 -2.53175 2.0266; 10.7117 -2.01895 2.07203; 10.758 -1.8533 2.05439; 10.8575 -1.33438 2.0938; 10.8887 -1.21894 2.09198; 10.8738 -1.2007 2.11695; 10.9057 -1.04908 2.13112; 10.9614 -0.765031 2.09502; 10.4173 -3.56214 2.10166; 10.8876 -1.35114 2.20572; 10.8994 -1.2599 2.16995; 10.8919 -1.16692 2.21074; 10.8921 -1.06507 2.20796; 10.8912 -0.999276 2.19963; 10.431 -3.73466 2.20401; 10.8596 -1.20284 2.35401; 10.8742 -1.18365 2.29397; 10.8794 -1.10899 2.29113] #################################### #### TODO stuff #################################### From 8fa2730dfeb0c9b29c721c2f11511701c847c38e Mon Sep 17 00:00:00 2001 From: GearsAD Date: Sat, 5 May 2018 10:55:18 -0500 Subject: [PATCH 25/27] Cleanup --- examples/marine/rov/highend/synchrony.jl | 34 ++---------------------- 1 file changed, 2 insertions(+), 32 deletions(-) diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl index 7e2bed640..24d7baaab 100644 --- a/examples/marine/rov/highend/synchrony.jl +++ b/examples/marine/rov/highend/synchrony.jl @@ -93,36 +93,6 @@ subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) println("[Caesar.jl] Running LCM listener") listener!(lcm_node) -#################################### -########### TESTING ################ -#################################### +println(" --- Done! Now we can run the solver on this dataset!") -using Unmarshal -packedJson = "{\"vecZij\":[-0.3171846270561218,-7.061707019805908,-0.14232116187023247],\"vecCov\":[0.01,0.0,0.0,0.0,0.01,0.0,0.0,0.0,0.01],\"nullhypothesis\":[0.5,0.5]}" -dictPacked = JSON.parse(packedJson) -packedType = Unmarshal.unmarshal(RoME.PackedPartialPose3XYYawNH, dictPacked) -prior = convert(RoME.PartialPose3XYYawNH, packedType) - -msg_points = [8.13305 -0.666686 -1.98914; 7.33001 -2.7746 -1.95458; 7.41093 -2.6327 -2.01238; 7.69368 -1.95501 -1.92476; 8.2546 -0.762656 -1.90386; 8.12886 -0.662197 -1.97778; 8.2284 -0.664559 -1.92912; 8.30724 -0.669406 -1.89989; 8.25277 -0.572092 -1.90335; 6.95415 -3.5615 -1.93757; 7.00676 -3.48989 -1.9279; 7.11229 -3.3879 -1.89566; 7.24134 -3.10426 -1.88373; 7.14833 -3.04169 -1.94523; 7.22599 -3.04247 -1.92337; 7.42208 -3.01578 -1.86684; 7.41939 -2.91612 -1.89005; 7.3394 -2.77466 -1.91547; 7.39894 -2.73838 -1.89973; 7.49792 -2.7474 -1.85854; 7.53263 -2.67156 -1.87655; 7.58568 -2.63219 -1.88512; 7.68323 -2.31885 -1.89035; 7.7267 -2.31875 -1.85706; 7.69877 -2.09793 -1.89411; 7.73164 -2.133 -1.87529; 7.85301 -2.11651 -1.84519; 7.69317 -2.03746 -1.89559; 7.72678 -2.06448 -1.83644; 7.79448 -1.97703 -1.86775; 7.91017 -1.9528 -1.84304; 8.41928 -0.840704 -1.82909; 8.34555 -0.726141 -1.81307; 8.39264 -0.749951 -1.84908; 8.34362 -0.67159 -1.86608; 8.42085 -0.671137 -1.84409; 8.33107 -0.572257 -1.87484; 8.46793 -0.532441 -1.87504; 8.86929 -0.410264 -1.82218; 7.11603 -3.68444 -1.81304; 7.21039 -3.6857 -1.79351; 7.18567 -3.625 -1.80003; 7.13474 -3.43812 -1.8485; 7.21834 -3.39527 -1.81634; 7.26278 -3.42481 -1.79259; 7.26514 -3.30744 -1.81573; 7.36375 -3.29841 -1.77822; 7.42148 -3.21786 -1.78483; 7.34848 -3.10449 -1.85252; 7.44674 -3.1279 -1.81243; 7.47251 -3.12847 -1.77622; 7.44678 -3.02855 -1.80531; 7.49123 -3.0391 -1.78801; 7.62013 -3.01816 -1.78377; 7.55001 -2.93322 -1.78222; 7.62267 -2.88699 -1.76532; 7.53798 -2.77578 -1.83574; 7.63435 -2.64456 -1.82538; 7.74163 -2.57217 -1.83817; 7.74155 -2.44163 -1.81968; 7.88203 -2.31833 -1.80369; 7.78274 -2.16268 -1.82694; 7.86112 -2.13914 -1.80183; 7.80445 -2.06348 -1.81023; 7.84831 -2.05659 -1.78562; 7.8813 -1.97617 -1.79964; 7.91913 -1.95288 -1.8011; 8.60678 -0.960857 -1.72939; 8.41351 -0.832237 -1.77241; 8.60636 -0.886577 -1.74823; 8.45035 -0.710839 -1.78875; 8.50181 -0.682496 -1.78679; 8.646 -0.660311 -1.75779; 8.70511 -0.668845 -1.73484; 8.97384 -0.411165 -1.74512; 8.71354 -0.29926 -1.73225; 8.79922 -0.081106 -1.79118; 8.93563 -0.0594269 -1.74559; 9.21225 0.23 -1.70039; 7.2188 -3.68633 -1.75465; 7.30254 -3.72104 -1.69494; 7.2783 -3.50739 -1.73681;7.39617 -3.52631 -1.69575; 7.31134 -3.45176 -1.75355; 7.39193 -3.43122 -1.70986; 7.46664 -3.43167 -1.68405; 7.36999 -3.37781 -1.74957; 7.47497 -3.35202 -1.70489; 7.40469 -3.26445 -1.76282; 7.4965 -3.23834 -1.73278; 7.58757 -3.2545 -1.69171; 7.5308 -3.11797 -1.75686; 7.59792 -3.13523 -1.71102; 7.66952 -3.13078 -1.68992; 7.5362 -3.04662 -1.74404; 7.624 -3.04898 -1.7097; 7.70336 -3.02919 -1.69376; 7.66977 -2.98466 -1.72511; 7.69759 -2.95148 -1.69861; 7.78195 -2.96759 -1.69854; 7.65377 -2.82563 -1.74274; 7.73278 -2.85684 -1.72134; 7.81868 -2.85701 -1.67667; 7.74381 -2.75828 -1.71499; 7.82201 -2.75884 -1.6817; 7.77398 -2.67167 -1.75801; 7.95052 -2.65058 -1.68134; 8.13994 -2.05906-1.65793; 8.3829 -1.48036 -1.70232; 8.51415 -1.45724 -1.67472; 8.62934 -1.08777 -1.68123; 8.69963 -1.06701 -1.65084; 8.60998 -0.97559 -1.688; 8.69745 -0.981553 -1.65182; 8.79554 -0.987026 -1.63386; 8.7319 -0.909788 -1.69191; 8.80595 -0.910251 -1.66289; 8.63669 -0.761817 -1.70364; 8.7746 -0.761474 -1.63402; 8.81152 -0.836932 -1.65123; 8.75597 -0.676409 -1.6982; 8.83655 -0.683122 -1.66625; 8.91034 -0.675144 -1.63164; 8.75458 -0.644994 -1.71352; 8.83507 -0.637578 -1.66737; 8.92332 -0.640372 -1.63939; 8.88873 -0.4995 -1.64875; 8.953 -0.472828 -1.65053; 9.21588 -0.407209 -1.70309; 8.83028 -0.300018 -1.68266; 8.93151 -0.13066 -1.62931; 9.11317 -0.0894114 -1.64828; 9.14727 -0.0504241 -1.66238; 9.2853 0.018847 -1.65123; 9.27841 0.100733 -1.63181; 9.38506 0.207492 -1.64225; 7.33163 -3.80425 -1.67524; 7.4505 -3.80611 -1.61566; 7.44236 -3.65921 -1.6304;7.46728 -3.62862 -1.65288; 7.41128 -3.52113 -1.65652; 7.49486 -3.52736 -1.61823; 7.51325 -3.44957 -1.62912; 7.58415 -3.42803 -1.62324; 7.50379 -3.36985 -1.66199; 7.59843 -3.36602 -1.62472; 7.66672 -3.31033 -1.60573; 7.62418 -3.2426 -1.66174; 7.68066 -3.25348 -1.6434; 7.657 -3.10878 -1.66591; 7.7299 -3.137 -1.63033; 7.80446 -3.12522 -1.60822; 7.65915 -3.03175 -1.66533; 7.74211 -3.04956 -1.65322; 7.82173 -3.04205 -1.62122; 7.94453 -3.0225 -1.58794; 7.77205 -2.95296 -1.6639; 7.80651 -2.95254 -1.63594; 7.89167 -2.97837 -1.59225; 7.83161 -2.85462 -1.64224; 7.8735 -2.74977 -1.64852; 7.96272 -2.77469 -1.61417; 8.01345 -2.75962 -1.61409; 7.96332 -2.65158 -1.663; 8.029 -2.67722 -1.6247; 8.13682 -2.64086 -1.59362; 8.1353 -2.57561 -1.59565; 8.23906 -2.57094 -1.58206; 8.28438 -2.1644 -1.59261; 8.18887 -2.10235 -1.64499; 8.24944 -2.08471 -1.60043; 8.33484-2.09048 -1.57878; 8.31898 -2.00616 -1.5851; 8.35917 -1.97498 -1.59124; 8.5274 -1.82453 -1.56482; 8.64767 -1.67922 -1.56243; 8.54714 -1.55463 -1.63745; 8.70146 -1.55133 -1.56993; 8.81851 -1.43037 -1.57818; 8.61639 -1.30276 -1.62393; 8.70436 -1.26571 -1.58612; 8.78891 -1.27392 -1.55777; 8.71293 -1.20614 -1.60376; 8.80067 -1.24169 -1.55108; 8.75034 -1.13227 -1.59358; 8.81222 -1.11705 -1.57819; 8.89147 -1.08098 -1.55968; 8.75996 -0.976659 -1.61642; 8.82927 -0.978143 -1.60558; 8.94628 -0.968476 -1.55149; 8.82372 -0.925277 -1.61951; 8.94282 -0.924494 -1.59144; 9.01365 -0.907619 -1.5636; 8.80925 -0.828699 -1.61766; 8.90396 -0.805497 -1.59344; 8.99768 -0.836899 -1.5561; 8.87327 -0.693405 -1.5989; 8.95383 -0.706185 -1.57566; 9.03817 -0.681261 -1.57155; 8.95894 -0.636554 -1.6068; 9.02782 -0.633858 -1.56722; 9.13775 -0.63882 -1.56831; 8.98859 -0.527506-1.58598; 9.25458 -0.440166 -1.56383; 9.39715 -0.413589 -1.57052; 9.08028 -0.301593 -1.58121; 9.30105 -0.0821714 -1.5894; 9.32646 -0.0825402 -1.54346; 9.31103 0.0188006 -1.60546; 9.36211 -0.0315956 -1.58255; 9.40859 0.0582638 -1.56565; 9.33724 0.164537 -1.60013; 7.41756 -3.88429 -1.58835; 7.49404 -3.88528 -1.56086; 7.59439 -3.80835 -1.54417; 7.58028 -3.76301 -1.56501; 7.6912 -3.75873 -1.52383; 7.63961 -3.64637 -1.56041; 7.68712 -3.66294 -1.52443; 7.70036 -3.55303 -1.50835; 7.60073 -3.47212 -1.57264; 7.73125-3.46561 -1.533; 7.80729 -3.41979 -1.51537; 7.72489 -3.39373 -1.55777; 7.80284 -3.36428 -1.52758; 7.87125 -3.36378 -1.50502; 7.81666 -3.28242 -1.54068; 7.8983 -3.25196 -1.52611; 7.84543 -3.18326 -1.57477; 7.93177 -3.16999 -1.53789; 7.98771 -3.15632 -1.50631; 7.85874 -3.0786 -1.57764; 7.9384 -3.09189 -1.53523; 8.01703 -3.03746 -1.52778; 7.94242 -2.96286 -1.55939; 8.02609 -2.96615 -1.52098; 8.10987 -2.99812 -1.51162; 7.96294 -2.85831 -1.57287; 8.06 -2.86351 -1.52783; 7.98697 -2.81714 -1.54739; 8.05934 -2.77438 -1.55267; 8.14631 -2.76685 -1.51493; 8.05811 -2.70943 -1.57238; 8.15005 -2.67439 -1.54221; 8.28452 -2.661 -1.50804; 8.17109 -2.60351 -1.56; 8.24861 -2.58834 -1.52185; 8.35741 -2.6009 -1.50883; 8.3326 -2.44045 -1.53432; 8.28463 -2.31734 -1.56894; 8.34984 -2.16709 -1.54679; 8.43378 -2.15613 -1.50469; 8.52773 -2.15523 -1.47357; 8.30183 -2.05684 -1.55211; 8.38868 -2.08524 -1.53649; 8.48414 -2.09002 -1.49077; 8.40321 -2.0011 -1.53912; 8.48467 -1.99489 -1.5068; 8.44261 -1.8805 -1.54025; 8.61797 -1.89365 -1.50339; 8.68828 -1.90051 -1.48136; 8.53031 -1.77303 -1.55471; 8.62317 -1.79284 -1.53347; 8.68308 -1.785 -1.49275; 8.67378 -1.71238 -1.50152; 8.7493 -1.67734 -1.4842; 8.70981 -1.57331 -1.51749; 8.78752 -1.51086 -1.50909; 8.75532 -1.37866 -1.53886; 8.81372 -1.41362 -1.49875; 8.97681 -1.44691 -1.46639; 8.82156 -1.32599 -1.50903; 8.92709 -1.31209 -1.47492; 8.84296 -1.24081 -1.50997; 8.90102 -1.20466 -1.4851; 8.8959 -1.13884 -1.52455; 9.03726 -1.13522 -1.47368; 9.07792 -1.11016 -1.47501; 9.03368 -0.993412 -1.51639; 9.12597 -1.02331 -1.47331; 9.05137 -0.9358 -1.5055; 9.14045 -0.912891 -1.48818; 9.19316 -0.911113 -1.47731; 9.05615 -0.835174 -1.52273; 9.11712 -0.836341 -1.47704; 9.21873 -0.833898 -1.44901; 9.03892 -0.718392 -1.51464; 9.1539 -0.706229 -1.49472; 9.20794 -0.693736 -1.45773; 9.1583 -0.647661 -1.52018; 9.26101 -0.653632 -1.47569; 9.33435 -0.639487 -1.4497; 9.40269 -0.663579 -1.44972; 9.07074 -0.555406 -1.51867; 9.18062 -0.490745 -1.5104; 9.26224 -0.505754 -1.47962; 9.29731 -0.474816 -1.51095; 9.40505 -0.414046 -1.52108; 9.61138 -0.415102 -1.45006; 9.71553 -0.411095 -1.47784; 9.25595 -0.302944 -1.48594; 9.2409 -0.130542 -1.48535; 9.4211 -0.0827164 -1.50821; 9.73354 0.164809 -1.45828; 9.8251 0.23 -1.44384; 9.8593 0.207764 -1.45978; 7.62942 -3.88704 -1.50066; 7.70896 -3.88809 -1.45025; 7.72487 -3.78679 -1.47784; 7.75605 -3.76038 -1.46928; 7.86776 -3.76214 -1.42379; 7.71889 -3.69544 -1.47467; 7.80191 -3.65841 -1.45691; 7.74748 -3.56872 -1.48355; 7.8039 -3.55084 -1.4551; 7.86507 -3.56979 -1.41651; 7.8109 -3.47855 -1.46521; 7.91315 -3.45867 -1.43884; 7.92538 -3.39063 -1.45731; 7.99033 -3.38372 -1.42551; 7.954 -3.27693 -1.47893; 8.00046 -3.27787 -1.45075; 8.10351 -3.24747 -1.41526; 8.01241 -3.16879 -1.47875; 8.14747 -3.16122 -1.4294; 8.05673 -3.06469 -1.48182; 8.133 -3.07893 -1.44455; 8.14941 -2.98352 -1.45302; 8.24555 -2.99722 -1.42521; 8.16319 -2.9239 -1.4745; 8.22432 -2.87983 -1.4422; 8.31108 -2.89397 -1.40592; 8.16073 -2.7793 -1.47788; 8.25412 -2.78809 -1.44755; 8.34077 -2.76129 -1.43168; 8.27055 -2.70966 -1.47384; 8.37037 -2.68255 -1.44861; 8.36185 -2.58153 -1.46068; 8.4685 -2.58894 -1.42574; 8.53497 -2.57002 -1.40068; 8.45105 -2.44018 -1.46202; 8.56557 -2.43996 -1.41001; 8.46258 -2.31691-1.46111; 8.55637 -2.31666 -1.42923; 8.63266 -2.31646 -1.39388; 8.58322 -2.16484 -1.4229; 8.49714 -2.05423 -1.44698; 8.59525 -2.08273 -1.43086; 8.65861 -2.11585 -1.40081;8.67609 -2.01421 -1.4132; 8.74035 -2.00187 -1.39731; 8.7028 -1.9114 -1.44236; 8.79662 -1.91038 -1.40894; 8.74193 -1.79138 -1.45802; 8.78986 -1.80376 -1.43111; 8.86592 -1.83737 -1.39425; 8.80818 -1.71824 -1.43973; 8.90449 -1.72161 -1.39564; 8.82366 -1.62996 -1.46048; 8.98643 -1.64979 -1.38163; 9.00804 -1.42194 -1.42528; 9.11259 -1.44395 -1.38835; 8.94866 -1.31938 -1.44049; 9.03378 -1.36176 -1.41113; 9.0982 -1.338 -1.39255; 9.04051 -1.25341 -1.43359; 9.10045 -1.24517 -1.38973; 9.17612 -1.22152 -1.36591; 9.05821 -1.16566 -1.45043; 9.11171 -1.14394 -1.41049; 9.16124 -1.03727 -1.44099; 9.24244 -1.05245 -1.40744; 9.2929 -1.07057 -1.37432; 9.15703 -0.965409 -1.44025; 9.25657 -0.933823 -1.42073; 9.34864 -0.944921 -1.37767; 9.43806 -0.924292 -1.37009; 9.24319 -0.827533 -1.41227; 9.3294 -0.833107 -1.38997; 9.43034 -0.885003 -1.36946; 9.25546 -0.738343 -1.41857; 9.34182 -0.720377 -1.39511; 9.42327 -0.696408 -1.37449; 9.37856 -0.661742 -1.41594; 9.44817 -0.665513 -1.37423; 9.54004 -0.639891 -1.37785; 9.34185 -0.565499 -1.39796; 9.44625 -0.555762 -1.36425; 9.55942 -0.43464 -1.43486; 9.7452 -0.421142 -1.38408; 9.8286 -0.0228573 -1.35198; 9.82844 0.154331 -1.41368; 9.92288 0.0954518 -1.34291;9.90217 0.23 -1.40229; 7.88147 -3.89034 -1.36964; 7.98954 -3.7877 -1.3587; 7.90168 -3.63439 -1.40918; 7.99498 -3.63537 -1.37955; 8.04855 -3.54296 -1.34061; 7.91717 -3.49871 -1.40182; 8.03693 -3.47506 -1.36851; 8.11423 -3.46524 -1.33564; 8.03048 -3.36531 -1.39959; 8.10995 -3.41316 -1.37204; 8.18832 -3.37913 -1.33439; 8.05925 -3.30357 -1.39284; 8.09722 -3.31346 -1.37357; 8.2279 -3.2827 -1.341; 8.3015 -3.28489 -1.31965; 8.24177 -3.19816 -1.34571; 8.29611 -3.23996 -1.31741; 8.35 -3.09338 -1.33829; 8.39175 -3.0973 -1.31292; 8.26362 -2.96658 -1.38953; 8.35598 -3.00774 -1.35367; 8.4177 -3.01966 -1.33592; 8.34203 -2.93658 -1.34356; 8.41807 -2.89459 -1.33133; 8.26445 -2.81937 -1.36981; 8.31499 -2.84199 -1.37649; 8.46734 -2.80265 -1.3194; 8.51493 -2.79455 -1.32239; 8.38928 -2.74193 -1.39083; 8.53433 -2.70854 -1.34475; 8.6111 -2.56986 -1.36511; 8.55654 -2.55126 -1.37422; 8.61519 -2.55108 -1.33598; 8.66242 -2.43975 -1.35405; 8.77324 -2.43958 -1.32211; 8.74682 -2.31617 -1.33942; 8.71132 -2.17388 -1.35847; 8.84283 -2.17269 -1.30538; 8.69707 -2.11977 -1.35836; 8.80198 -2.11206 -1.33891; 8.90996 -2.10415 -1.29518; 8.70858 -2.05141 -1.3385; 8.82067 -2.01068 -1.34296; 8.9311 -2.02181 -1.29465; 8.82262 -1.92134 -1.34516; 8.86836 -1.9435 -1.37192; 8.87987 -1.85412 -1.3607; 9.00311 -1.72521 -1.345; 9.03658 -1.64471 -1.33844; 9.18546 -1.55211 -1.29935; 9.14454 -1.45338 -1.34652; 9.15242 -1.36976 -1.33972; 9.18552 -1.33068 -1.34196; 9.14017 -1.24487 -1.35811; 9.19789 -1.25381 -1.32171; 9.31812 -1.23959 -1.28899; 9.30513 -1.13909 -1.33633; 9.40745 -1.13647 -1.29939; 9.36927 -1.06863 -1.33437; 9.39119 -1.0486 -1.31594; 9.35264 -0.979259 -1.35469; 9.43154 -0.95339 -1.32713; 9.45719 -0.832107 -1.33176; 9.45681 -0.726692 -1.33431; 9.53888 -0.747489 -1.2895; 9.55472 -0.658639 -1.33139; 9.64481 -0.652237 -1.31575; 9.54654 -0.556181 -1.3173; 9.64632 -0.575598 -1.2726; 9.67597 -0.503081 -1.27051; 9.67082 -0.305563 -1.31705; 9.75302 -0.13027 -1.25295; 10.1146 -0.00188613 -1.26858; 9.94753 0.144026 -1.31828; 10.0769 0.122956 -1.26962; 10.0377 0.207972 -1.32521; 10.0907 0.23 -1.32146; 7.99695 -3.81484 -1.32121; 8.10572 -3.783 -1.28751; 8.17481 -3.80469 -1.25325; 8.11985 -3.70831 -1.28876; 8.19635 -3.67064 -1.2556; 8.12309 -3.5842 -1.29837; 8.19825 -3.5838 -1.25423; 8.1474 -3.49073 -1.28894; 8.22659 -3.48138 -1.26825; 8.29591 -3.49193 -1.24322; 8.25398 -3.39082 -1.29193; 8.31934 -3.40568 -1.26631; 8.38905 -3.38715 -1.24853; 8.26628 -3.3146 -1.30824; 8.34161 -3.29156 -1.26788; 8.33949 -3.21519 -1.28758; 8.43685 -3.21624 -1.25472; 8.50179 -3.19176 -1.22664; 8.37567 -3.11798 -1.29999; 8.45543 -3.11923 -1.27612; 8.54946 -3.07889 -1.26931; 8.45996 -3.02107 -1.28303; 8.54519 -3.02266 -1.25773; 8.63731 -3.03535 -1.23309; 8.45677-2.93512 -1.28541; 8.56506 -2.90767 -1.25033; 8.55518 -2.82612 -1.25133; 8.65478 -2.7821 -1.252; 8.59958 -2.74268 -1.28187; 8.65561 -2.72355 -1.26215; 8.73004 -2.72658 -1.23358; 8.68441 -2.63976 -1.26496; 8.83945 -2.64933 -1.2254; 8.86184 -2.56907 -1.20895; 8.84909 -2.43942 -1.28455; 8.87241 -2.31586 -1.26078; 8.99454 -2.31555 -1.20363; 8.82163 -2.15212 -1.28116; 8.90127 -2.17213 -1.26516; 8.95957 -2.11698 -1.25033; 8.89702 -2.02466 -1.27242; 8.98847 -2.04777 -1.23233; 9.07635 -2.00573 -1.22246; 9.04403 -1.94195 -1.25051; 9.11725 -1.96288 -1.21892; 9.18591 -1.69038 -1.26487; 9.25522 -1.62661 -1.2213; 9.35661 -1.61902 -1.18898; 9.35459 -1.54849 -1.22062; 9.37907 -1.49911 -1.22833; 9.41445 -1.43734 -1.25213; 9.50064 -1.4454 -1.19055; 9.45287 -1.35588 -1.2095; 9.51432 -1.34027 -1.1967; 9.44188 -1.23693 -1.21805; 9.51509 -1.21339 -1.20797; 9.43015 -1.14994 -1.23649; 9.53327 -1.12329 -1.21565; 9.5575 -1.05411 -1.21583; 9.64671 -0.942096 -1.25251; 9.7221 -0.939728 -1.20215; 9.66266 -0.825586 -1.18822; 9.74811 -0.893356 -1.20708; 9.76439 -0.758315 -1.17943; 9.80256 -0.665151 -1.24611; 9.86111 -0.665142 -1.20663; 9.9182 -0.648438 -1.18531; 9.67681 -0.55696 -1.25323; 9.88191 -0.547346 -1.16921; 9.86273 -0.493807 -1.19676; 9.96216 -0.451374 -1.19648; 10.0116 -0.417788 -1.24075; 10.1019 -0.416416 -1.23101; 10.1573 -0.415088 -1.21675; 9.8407 -0.306774 -1.23427; 9.96904 -0.307435 -1.19664; 9.86401 -0.130058 -1.21436; 9.9574 -0.13 -1.17264; 10.1522 -0.053565 -1.16365; 10.1282 0.0397982 -1.24595; 10.2048 0.0193841 -1.18396; 10.1985 0.11354 -1.23305; 10.2436 0.23 -1.23262; 8.17166 -3.89417 -1.18918; 8.26415 -3.89536 -1.15539; 8.22316 -3.81827 -1.21798; 8.28595 -3.79644 -1.19171; 8.37809 -3.80543 -1.14508; 8.30134 -3.71474 -1.18533; 8.3827 -3.70659 -1.17136; 8.44784 -3.64089 -1.16989; 8.26316 -3.54506 -1.19109; 8.34649 -3.50134 -1.18692; 8.39575 -3.46969 -1.19697; 8.41082 -3.40649 -1.20352; 8.48437 -3.36932 -1.19141; 8.44782 -3.31114 -1.20817; 8.50316 -3.29661 -1.19529; 8.49683 -3.24139 -1.20189; 8.611 -3.22235 -1.15257; 8.56361 -3.1422 -1.20928; 8.63891 -3.13077 -1.17367; 8.73142 -3.14417 -1.13518; 8.58758 -3.01893 -1.20448; 8.6633 -3.02797 -1.18242; 8.75361 -3.0264 -1.14989; 8.67167 -2.92945 -1.17415; 8.75574 -2.92249 -1.13492; 8.67429 -2.83849 -1.17924; 8.75053 -2.78264 -1.19336; 8.75067 -2.74323 -1.20711; 8.90114 -2.74377 -1.12932; 8.9355 -2.71026 -1.14417; 8.89448 -2.65004 -1.17286; 8.96761 -2.62736 -1.13739; 8.90051 -2.5504 -1.18336; 8.94086 -2.55017 -1.13999; 9.06578 -2.14969 -1.16672; 9.06404 -2.04676 -1.19299; 9.22338 -1.7635 -1.1736; 9.444 -1.75037 -1.1073; 9.33908 -1.67698 -1.17859; 9.43284 -1.64101 -1.14029; 9.47244 -1.61665 -1.10086; 9.45063 -1.55741 -1.15246; 9.53428 -1.53377 -1.11762; 9.52224 -1.4755 -1.14419; 9.61109 -1.45997 -1.11779; 9.55052 -1.35552 -1.14028; 9.6156 -1.37021 -1.11838; 9.69834 -1.41132 -1.09685; 9.55376 -1.28834 -1.15386; 9.62763 -1.23976 -1.13605; 9.61076 -1.16764 -1.15094; 9.72468 -1.12907 -1.0846; 9.64707 -1.06473 -1.1646; 9.74008 -1.0748 -1.13258; 9.82232 -1.03712 -1.10856; 9.77841 -0.943979 -1.15026; 9.81616 -0.947311 -1.12722; 9.78458 -0.843775 -1.16331; 9.85144 -0.886602 -1.1344; 9.95909 -0.887349 -1.09865; 9.8498 -0.779165 -1.12974; 9.92097 -0.750475 -1.09721; 9.88447 -0.672461 -1.15193; 9.95051 -0.66987 -1.13148; 10.0349 -0.665737 -1.091; 9.91181 -0.57641 -1.12112; 9.9741 -0.565906 -1.10512; 10.0322 -0.557936 -1.08372; 9.98071 -0.494436 -1.13086; 10.0717 -0.466237 -1.10687; 10.1262 -0.130059 -1.08455; 10.2103 -0.0839027 -1.10359; 10.295 -0.0638901 -1.07009; 10.2453 0.039848 -1.15115; 10.3153 -0.0124913 -1.09387; 10.4115 0.019487 -1.06954; 10.4129 0.219116 -1.14779; 8.32157 -3.89613 -1.11681; 8.37881 -3.89689 -1.07766; 8.44419 -3.80656 -1.10665; 8.50054 -3.80579 -1.07599; 8.4334 -3.72562 -1.11094; 8.5045 -3.70977 -1.06991; 8.59151 -3.67675 -1.05017; 8.41991 -3.55676 -1.12044; 8.70585 -3.28828 -1.045; 8.66851 -3.22321 -1.11222; 8.75056-3.25548 -1.06756; 8.75835 -3.12319 -1.11044; 8.85612 -3.12477 -1.04741; 8.91929 -3.12479 -1.03573; 8.75757 -3.03328 -1.1131; 8.85732 -3.03673 -1.07033; 8.95723 -3.03543 -1.06226; 8.76357 -2.93038 -1.0959; 8.86112 -2.94811 -1.07233; 8.9378 -2.90039 -1.06428; 8.78361 -2.83451 -1.11314; 8.86281 -2.8389 -1.05817; 8.96642 -2.84558 -1.05303; 8.97096 -2.75428 -1.07534; 9.08488 -2.63939 -1.06917; 9.05244 -2.56843 -1.08573; 9.48168 -1.81375 -1.06256; 9.44902 -1.76041 -1.04792; 9.55354 -1.76228 -1.01998; 9.45634 -1.66418 -1.08521; 9.52515 -1.65549 -1.0481; 9.60344 -1.64321 -1.0246; 9.60205 -1.57375 -1.05106; 9.62972 -1.45287 -1.05376; 9.72043 -1.44075 -1.03361; 9.65117 -1.35867 -1.06208; 9.73841 -1.36487 -1.02511; 9.81332 -1.40898 -1.0037; 9.63962 -1.26313 -1.07301; 9.70885 -1.26152 -1.02821; 9.75727 -1.17843 -1.05505; 9.85652 -1.07044 -1.05251; 9.92729 -1.11395 -0.996764; 9.88122 -0.957964 -1.06081; 9.94558 -0.96789 -1.03345; 10.0101 -0.958689 -1.01416; 9.96055 -0.899567 -1.04588; 10.0335 -0.894606 -1.01414; 9.90412 -0.819615 -1.06992; 9.95568 -0.78675 -1.0397; 10.0615 -0.784521 -0.994391; 9.90072 -0.724492 -1.06989; 10.0103 -0.686742 -1.05015; 10.0705 -0.677452 -1.03337; 10.1414 -0.664146 -0.998512; 9.98994 -0.576773 -1.04985; 10.082 -0.607891 -1.01623; 10.13 -0.558442 -0.986296; 10.0198 -0.505125 -1.0476; 10.0883 -0.503572 -1.02874; 10.2969 -0.420021 -1.05578; 10.382 -0.418505 -1.03596; 10.1984 -0.309595 -1.03337; 10.2216 -0.130372 -1.01329; 10.3305 -0.0854991 -1.04587; 10.3534 -0.0792963 -1.00234; 10.4804 -0.0644978 -0.978096; 10.4003 -0.0344564 -1.03462; 10.4854 0.00486674 -1.00403; 10.4365 0.0878209 -1.02932; 10.5488 0.123682 -0.992736; 10.565 0.0971172 -0.980269; 10.5491 0.197908 -1.01938; 10.6233 0.165857 -0.969248; 8.45337 -3.89786 -1.03967; 8.54736 -3.89911 -0.979667; 8.55353 -3.82369 -1.02655; 8.61725 -3.79894 -0.992154; 8.6174 -3.71186 -0.99995;8.63164 -3.55334 -1.00295; 8.73188 -3.50641 -0.958243; 8.74545 -3.40985 -1.00052; 8.79638 -3.48086 -0.964719; 8.78404 -3.37203 -1.03742; 8.83771 -3.24704 -0.995056; 8.94751 -3.23663 -0.958078; 8.86821 -3.14651 -1.00694; 8.9556 -3.13684 -0.970885; 9.02286 -3.13723 -0.960914; 8.96751 -3.05114 -1.00487; 9.02928 -3.03785 -0.966687; 8.89511 -2.98206 -0.997659; 8.94613 -2.92712 -1.01427; 9.05697 -2.93395 -0.95936; 8.97041 -2.85698 -1.00518; 9.06493 -2.84679 -0.962328; 9.01754 -2.76463 -1.01925; 9.09586 -2.74276 -0.981077; 9.15212 -2.76494 -0.963107; 9.10982 -2.64847 -1.01343; 9.17817 -2.6639 -0.972843; 9.40488 -2.02945 -0.992509; 9.51284 -1.93748 -1.00345; 9.5796 -1.97591 -0.937923; 9.5221 -1.86955 -1.00313; 9.56245 -1.78109 -0.967533; 9.6221 -1.76599 -0.959151; 9.6993 -1.75188 -0.922306; 9.63333 -1.67164 -0.980384; 9.71204 -1.70345 -0.932278; 9.67836 -1.55262 -0.996402; 9.68023 -1.62348 -0.955817; 9.76187 -1.45006 -0.962577; 9.83974 -1.50824 -0.914302; 9.78087 -1.42962 -0.957879; 9.8289 -1.36021 -0.952937; 9.91766 -1.41669 -0.912264; 9.84649 -1.2732 -0.966831; 9.91214 -1.33157 -0.92461; 9.86194 -1.19319 -0.984987; 9.93074 -1.19164 -0.938272; 9.90394 -1.05555 -0.980985; 9.94916 -1.1021 -0.932343; 10.0399 -1.05186 -0.939811; 9.99032 -0.979262 -0.975814; 10.0453 -0.975398 -0.944891; 10.065 -0.91334 -0.95432; 10.1596 -0.897872 -0.919616; 10.0813 -0.797227 -0.940621; 10.1458 -0.788374 -0.915637; 10.0691 -0.73043 -0.971834; 10.1539 -0.692271 -0.943144; 10.2427 -0.668067 -0.918011; 10.1822 -0.615798 -0.940222; 10.257 -0.613929 -0.90503; 10.1875 -0.505976 -0.956471; 10.2924 -0.51048 -0.918316; 10.396 -0.482567 -0.907209; 10.3968 -0.431098 -0.923684; 10.502 -0.418774 -0.949609; 10.3046 -0.310833 -0.934475; 10.3166 -0.130705 -0.9406; 10.401 -0.108662 -0.918108; 10.4856 -0.0822289 -0.901025; 10.4998 0.00467861 -0.94002; 10.603 -0.0249176 -0.89401; 10.6554 0.0795156 -0.952705; 10.6528 0.187375 -0.942993; 10.7716 0.20861 -0.90758; 8.65852 -3.90056 -0.919711; 8.65931 -3.80363 -0.939144; 8.73904 -3.81367 -0.909819; 8.72662 -3.73178 -0.906897; 8.74441 -3.67971 -0.923739; 8.84116 -3.66396 -0.882514; 8.83119 -3.56046 -0.880003; 8.87665 -3.46233 -0.852428; 8.92448 -3.4244 -0.896218; 8.94726 -3.34318 -0.895364; 9.02176 -3.31073 -0.854148; 8.94306 -3.27657 -0.909451; 9.02363 -3.23766 -0.89206; 9.08127 -3.12797 -0.899115; 9.06867 -3.03458 -0.899412; 9.14179 -3.05035 -0.911603; 9.10321 -2.97921 -0.906559; 9.14545 -2.9512 -0.871645; 9.10385 -2.83713 -0.903954; 9.16189 -2.86971 -0.864585; 9.18324 -2.77002 -0.887914; 9.2963 -2.75544 -0.860368; 9.20605 -2.64807 -0.923987; 9.26103 -2.66951 -0.892907; 9.36145 -2.66463 -0.851068; 9.29848 -2.60159 -0.90041; 9.59132 -1.99326 -0.903305; 9.71844 -1.93564 -0.867248; 9.64012 -1.86892 -0.859924; 9.67297 -1.84669 -0.901293; 9.72223 -1.77178 -0.848821; 9.71525 -1.70387 -0.882441; 9.83884 -1.66789 -0.841803; 9.83607 -1.59295 -0.856215; 9.86632 -1.52699 -0.863218; 9.91405 -1.47896 -0.837141; 9.82725 -1.35505 -0.869998; 9.95032 -1.37674 -0.857743; 9.93759 -1.28321 -0.869967; 9.9449 -1.1948 -0.873188; 10.0085 -1.22443 -0.835557; 10.0578 -1.08388 -0.861096; 10.116 -1.09985 -0.849988; 10.0911 -1.02039 -0.873993; 10.1367 -1.00225 -0.851972; 10.2178 -1.01688 -0.829521; 10.1837 -0.927265 -0.861655; 10.2534 -0.903807 -0.836213; 10.1797 -0.811264 -0.862514; 10.2412 -0.794466 -0.827167; 10.0952 -0.726247 -0.87528; 10.2084 -0.70877 -0.864871; 10.278 -0.702075 -0.843668; 10.3402 -0.677216 -0.830273; 10.3042 -0.627884 -0.835242; 10.3263 -0.493717 -0.851925; 10.3715 -0.508342 -0.810803; 10.4559 -0.441632 -0.841572; 10.4427 -0.311854 -0.863554; 10.4995 -0.312497 -0.812518; 10.4868 -0.113362 -0.83724; 10.562 -0.0871804 -0.817783; 10.6167 -0.0458553 -0.823884; 10.8154 0.208784 -0.797257; 8.8226 -3.81541 -0.830979; 8.90019 -3.82991 -0.772084; 8.91869 -3.74701 -0.806164; 8.8851 -3.64767 -0.836407; 8.90953 -3.67112 -0.808496; 8.88186 -3.56714 -0.825452; 8.93591 -3.54554 -0.79489; 8.95768 -3.46156 -0.808402; 8.99491 -3.39319 -0.840133; 9.05349 -3.3365 -0.803314; 9.05558 -3.26883 -0.829553; 9.1153 -3.29161 -0.792029; 9.17282 -3.15061 -0.804655;9.22315 -3.12999 -0.768494; 9.16703 -3.10823 -0.826335; 9.2687 -3.05842 -0.786002; 9.18261 -2.96555 -0.814524; 9.27214 -2.97593 -0.776096; 9.20778 -2.83805 -0.814116; 9.27484 -2.88197 -0.780705; 9.33374 -2.86999 -0.756945; 9.28113 -2.78565 -0.808375; 9.39435 -2.76602 -0.776369; 9.42824 -2.67873 -0.768158; 9.45088 -2.66437 -0.769547; 9.76287 -1.89074 -0.811718; 9.8237 -1.85637 -0.763148; 9.85599 -1.76635 -0.765608; 9.91409 -1.80958 -0.736477; 9.86426 -1.70105 -0.787734; 9.9204 -1.69107 -0.758062; 9.88871 -1.60815 -0.775523; 9.93645 -1.5909 -0.766954; 9.95242 -1.51971 -0.761948; 9.95343 -1.44627 -0.76747; 10.0103 -1.35094 -0.777991; 10.19 -1.09817 -0.801455; 10.2638 -1.09651 -0.752168; 10.2013 -1.04822 -0.790719; 10.2688 -1.03282 -0.758536; 10.3479 -1.00319 -0.735953; 10.2993 -0.917263 -0.763131; 10.3521 -0.913453 -0.730471; 10.2747 -0.825629 -0.761552; 10.3372 -0.826967 -0.732576; 10.2794 -0.741207 -0.764131; 10.3644 -0.718684 -0.7463; 10.4524 -0.684504 -0.742296; 10.4138 -0.621878 -0.747992; 10.4874 -0.654941 -0.731342; 10.4008 -0.523771 -0.761516; 10.4847 -0.501098 -0.720489; 10.4988 -0.442658 -0.733906; 10.4858 -0.313042 -0.756153; 10.5947 -0.313772 -0.707634; 10.5089 -0.132129 -0.735844; 10.8294 0.0661929 -0.738347; 10.8459 0.149717 -0.727051; 10.9071 0.208862 -0.746507; 10.9337 0.222983 -0.703313; 8.88263 -3.90357 -0.747131; 8.9645 -3.86782 -0.71608; 9.0044 -3.83175 -0.698164; 8.95388 -3.77174 -0.748369; 9.018 -3.77455 -0.699861; 8.96524 -3.66655 -0.747572; 9.05587 -3.66778 -0.70463; 9.0362 -3.56199 -0.723803; 9.07057 -3.47531 -0.707329; 9.10666 -3.43301 -0.723133; 9.22036 -3.3586 -0.700726; 9.13606 -3.29191 -0.74447; 9.26453 -3.26214 -0.701581; 9.30301 -3.15677 -0.712248; 9.33438 -3.13183 -0.675994; 9.30329 -3.08301 -0.723214; 9.36537 -3.05575 -0.696422; 9.29901 -2.98205 -0.715471; 9.37051 -2.93705 -0.684343; 9.29275 -2.88735 -0.703457; 9.39549 -2.8568 -0.680111; 9.41128 -2.76594 -0.718066; 9.50192 -2.76636 -0.672918; 9.49342 -2.69056 -0.710823; 9.55748 -2.70987 -0.651009; 9.52123 -2.62825 -0.721525; 9.58702 -2.55715 -0.690026; 9.86504 -1.98592 -0.673386; 9.83904 -1.90104 -0.719018; 9.91467 -1.81298 -0.670913; 9.95498 -1.6995 -0.67985; 9.9998 -1.70629 -0.667584; 9.97282 -1.61201-0.69574; 10.0008 -1.61701 -0.664847; 9.98239 -1.54641 -0.705467; 10.0556 -1.54487 -0.656481; 10.1209 -1.44286 -0.617489; 10.2947 -1.1164 -0.677017; 10.3758 -1.01655 -0.649199; 10.3222 -0.92653 -0.674309; 10.3838 -0.917728 -0.660081; 10.4392 -0.917717 -0.6249; 10.3958 -0.825912 -0.64852; 10.4716 -0.876596 -0.618433; 10.3741 -0.749271 -0.654858; 10.4863 -0.703296 -0.649906; 10.5666 -0.692841 -0.647236; 10.4977 -0.641225 -0.657731; 10.5508 -0.633645 -0.600372; 10.5148 -0.524156 -0.647847; 10.5527 -0.579073 -0.603485; 10.8461 -0.0366059 -0.62071; 10.8965 0.0890991 -0.671638; 10.9111 0.145756 -0.636962; 11.0032 0.23 -0.627441; 9.05212 -3.86213 -0.626384; 9.10368 -3.83389 -0.589752; 9.06018 -3.76764 -0.642721; 9.12407 -3.7716 -0.59039; 9.06214 -3.68591 -0.658071; 9.14087 -3.66355 -0.627186; 9.07268 -3.56595 -0.643912; 9.16869 -3.56671 -0.599734; 9.15869 -3.48356 -0.627338; 9.25066 -3.46457 -0.580898; 9.29063 -3.38358 -0.613651; 9.28096 -3.28903 -0.63359; 9.32463 -3.26972 -0.601616; 9.29968 -3.15337 -0.649159; 9.39529 -3.16846 -0.601045; 9.38991 -3.1119 -0.641869; 9.44648 -3.07255 -0.603698; 9.40183 -2.96843 -0.629518; 9.46983 -2.98976 -0.596485; 9.39433 -2.90595 -0.607982; 9.51461 -2.86178 -0.599598; 9.48489 -2.82916 -0.60647; 9.58222 -2.76669 -0.59228; 9.64774 -2.74595 -0.592537; 9.59634 -2.6838 -0.601971; 9.66874 -2.67814 -0.552376; 9.68725 -2.6276-0.576941; 9.72647 -2.54723 -0.553012; 9.90758 -2.07151 -0.575054; 9.93652 -2.00636 -0.577217; 9.96196 -1.92214 -0.597176; 9.96892 -1.80291 -0.603707; 10.0738 -1.82617 -0.546124; 10.0748 -1.73151 -0.567892; 10.1148 -1.6971 -0.563558; 10.0881 -1.62395 -0.56572; 10.14 -1.53243 -0.557324; 10.2666 -1.40015 -0.553873; 10.2358 -1.34634 -0.521858; 10.3241 -1.33358 -0.522935; 10.3068 -1.23824 -0.540995; 10.328 -1.11674 -0.542055; 10.4274 -1.04352 -0.529785; 10.4623 -1.02143 -0.553902; 10.4175 -0.930838 -0.553472; 10.4655 -0.916603 -0.55359; 10.5632 -0.935223 -0.515087; 10.4343 -0.827223 -0.573108; 10.4815 -0.847226 -0.545281; 10.5471 -0.876398 -0.510695; 10.43 -0.760541 -0.591183; 10.4758 -0.736373 -0.560828; 10.5851 -0.71084 -0.515902; 10.5253 -0.681417 -0.568529; 10.5821 -0.65873 -0.537153; 10.6594 -0.645196 -0.528633; 10.5875 -0.570087 -0.537391; 10.6791 -0.458587 -0.544458; 10.775 -0.425652 -0.533426; 10.8981 -0.423825 -0.552032; 10.8411 -0.0978789 -0.571274; 10.8411 -0.0888615 -0.582055; 10.9249 -0.0264893 -0.546548; 10.9261 0.0192926 -0.545198; 11.017 0.0579211 -0.523436; 11.0327 0.114633 -0.509409; 11.0247 0.219561 -0.546751; 9.13401 -3.88286 -0.542171; 9.23566 -3.83606 -0.512983; 9.16565 -3.7727 -0.543084; 9.22428 -3.79056 -0.486523; 9.1724 -3.70597 -0.543871; 9.22707 -3.66529 -0.532484; 9.18839 -3.56721 -0.551451; 9.23591 -3.55989 -0.514824; 9.18098 -3.51176 -0.555283; 9.25142 -3.4944 -0.51626; 9.33877 -3.45711 -0.521174; 9.36724 -3.4005 -0.520207; 9.37316 -3.2872 -0.524639; 9.43731 -3.27961 -0.494252; 9.41737 -3.23522 -0.486149; 9.44297 -3.18611 -0.511249; 9.42923 -3.13417 -0.530244; 9.54554 -3.08516 -0.501576; 9.47091 -3.00829 -0.551036; 9.57903 -2.95621 -0.529331; 9.54007 -2.8522 -0.533218; 9.60488 -2.88915 -0.501066; 9.61311 -2.80917 -0.511008; 9.66146 -2.77383 -0.514546; 9.72326 -2.67082 -0.530536; 9.69106 -2.59987 -0.519396; 9.78343 -2.6097 -0.496791; 9.85339 -2.54673 -0.45387; 9.96194 -2.08105 -0.526526; 9.98992 -2.10501 -0.473729; 9.96532 -2.01189 -0.507028; 10.0741 -1.98847 -0.461177; 10.0403 -1.92805 -0.501399; 10.0763 -1.84426 -0.479384; 10.1206 -1.80831 -0.481322; 10.095 -1.67432 -0.518906; 10.1296 -1.72665 -0.481059; 10.1748 -1.62399 -0.483968; 10.2036 -1.54187 -0.502278; 10.2703 -1.49952 -0.486091; 10.2839 -1.43934 -0.516859; 10.3567 -1.43306 -0.45851; 10.2731 -1.34561 -0.469004; 10.3504 -1.29875 -0.464616; 10.3105 -1.21846 -0.475187; 10.3641 -1.2166 -0.449772; 10.389 -1.12929 -0.467602; 10.4842 -1.09972 -0.453199; 10.5035 -1.0298 -0.46109; 10.5095 -0.925923 -0.449358; 10.562 -0.913069 -0.474072; 10.5235-0.875823 -0.493299; 10.5755 -0.847254 -0.444991; 10.6486 -0.874719 -0.442086; 10.5152 -0.745579 -0.474602; 10.6098 -0.72881 -0.441183; 10.668 -0.704612 -0.472231; 10.6269 -0.67736 -0.462538; 10.6845 -0.656031 -0.441673; 10.6837 -0.549441 -0.44047; 10.7252 -0.47774 -0.45695; 10.832 -0.436446 -0.417683; 10.9006 -0.42435 -0.494748; 10.9906 -0.425173 -0.440588; 10.817 -0.316434 -0.493608; 10.8399 -0.133414 -0.473096; 10.9478 -0.0790336 -0.440996; 11.0012 -0.0271278 -0.405769; 11.0734 0.0451085 -0.418459; 11.1224 0.142695 -0.442441; 11.134 0.219649 -0.464518; 11.2241 0.21969 -0.409383; 9.15957 -3.90736 -0.469634; 9.25841 -3.85499 -0.429155; 9.32101 -3.83809 -0.39955; 9.27863 -3.79779 -0.420027; 9.33436 -3.76469 -0.383557; 9.27622 -3.69747 -0.452286; 9.34542 -3.65553 -0.419123; 9.2856 -3.56843 -0.456056; 9.3708 -3.56123 -0.41958; 9.41806 -3.58524 -0.387678; 9.37675 -3.49567 -0.442354; 9.43548 -3.4844 -0.396513; 9.41001 -3.40734 -0.467251; 9.45519 -3.40614 -0.417845; 9.45388 -3.29184 -0.424363; 9.47167 -3.23612 -0.437456; 9.56918 -3.18272 -0.421374; 9.59219 -3.10925 -0.429349; 9.67667 -3.08344 -0.403573; 9.59387 -2.96852 -0.434853; 9.66496 -3.02152 -0.416208; 9.62622 -2.89677 -0.400081; 9.69332 -2.89152 -0.366005; 9.6992 -2.80242 -0.394381; 9.77776 -2.76767 -0.362737; 9.85018 -2.63657 -0.440492; 9.97502 -2.43629 -0.366159; 9.96797 -2.31336 -0.415605; 10.0396 -2.31319 -0.364445; 10.0502 -2.12313 -0.395372; 10.0894 -2.08957 -0.373141; 10.0658 -2.02509 -0.37354; 10.0918 -2.0323 -0.348432; 10.0759 -1.95349 -0.423612; 10.1568 -1.93258 -0.389476; 10.1579 -1.83671 -0.394966; 10.2208 -1.82539 -0.363142; 10.163 -1.71146 -0.411641; 10.2315 -1.73675 -0.360608; 10.2614 -1.62783 -0.380429; 10.3083 -1.4989 -0.405604; 10.3309 -1.5393 -0.370478; 10.4198 -1.51594 -0.353024; 10.3875 -1.43364 -0.372789; 10.4397 -1.40652 -0.373154; 10.3851 -1.32754 -0.387951; 10.4347 -1.34206 -0.364522; 10.4101 -1.22638 -0.374471; 10.4665 -1.23764 -0.342884; 10.5187 -1.10276 -0.381078; 10.5569 -1.12557 -0.343408; 10.5215 -0.999399 -0.412178; 10.564 -1.03515 -0.360002; 10.5259 -0.916759 -0.405869; 10.5916 -0.919947 -0.367008; 10.6131 -0.857335 -0.366437; 10.6539 -0.851079 -0.362332; 10.6271 -0.746216 -0.355702; 10.7258 -0.703061 -0.364764; 10.7349 -0.658854 -0.360367; 10.7315 -0.55736 -0.347733; 10.7763 -0.544265 -0.352559; 10.7479 -0.489967 -0.356104; 10.8291 -0.475586 -0.357747; 11.0468 -0.42641 -0.326376; 10.8922 -0.318025 -0.353456; 10.9554 -0.099999 -0.346942; 11.0013 -0.0583439 -0.389831; 11.126 -0.0273701 -0.351725; 11.0735 0.0192046 -0.37782; 11.1455 0.0192098 -0.321653; 11.2073 0.0402121 -0.340988; 11.0773 0.094468 -0.383908; 11.1634 0.129231 -0.328862; 11.2223 0.141244 -0.342217; 11.1902 0.209478 -0.354315; 11.2094 0.20957 -0.296154; 9.30444 -3.83842 -0.34937; 9.32073 -3.90952 -0.349854; 9.36666 -3.81484 -0.320111; 9.37005 -3.69958 -0.354802; 9.41774 -3.65713 -0.320255; 9.40024 -3.56384 -0.336953; 9.4463 -3.57622 -0.331685; 9.47226 -3.49578 -0.331204; 9.535 -3.47073 -0.291814; 9.49175 -3.40837 -0.368557; 9.56566 -3.40926 -0.318625; 9.49773 -3.31171 -0.348818; 9.59434 -3.27983 -0.320825; 9.64734 -3.28817 -0.300255; 9.60318 -3.1984 -0.348311; 9.68166 -3.20004 -0.298423; 9.6893 -3.09489 -0.32597; 9.776 -3.12598 -0.299906; 9.70061 -3.00429 -0.329133; 9.77493 -3.04837 -0.309153; 9.71977 -2.91212 -0.29993; 9.75535 -2.91006 -0.313941; 9.68724 -2.8425 -0.333163; 9.7858 -2.8159 -0.290334; 9.86666 -2.76807 -0.312336; 9.83238 -2.73378 -0.326506; 9.86917 -2.6896 -0.313544; 9.9292 -2.63016 -0.300365; 9.94467 -2.54601 -0.324965; 9.99699 -2.56401 -0.278673; 10.0824 -2.43595 -0.261875; 10.0588 -2.31321 -0.284891; 10.0795 -2.16064 -0.341524; 10.1274 -2.12275 -0.291957; 10.0929 -2.03216 -0.295061; 10.1359 -2.0684 -0.290021; 10.2053 -1.99769 -0.284405; 10.1139 -1.95331 -0.317806; 10.2205 -1.93892 -0.277415; 10.1951 -1.83674 -0.315092; 10.2356 -1.84007 -0.26516; 10.2504 -1.74033 -0.294437; 10.3213 -1.73914 -0.267677; 10.2956 -1.67087 -0.282066; 10.3457 -1.62068 -0.282245; 10.3855 -1.53825 -0.289448; 10.4214 -1.51614 -0.270344; 10.3859 -1.47809 -0.271088; 10.4733 -1.42922 -0.2635; 10.4102 -1.34291 -0.28123; 10.454 -1.33114 -0.30695; 10.4205 -1.28575 -0.273282; 10.5618 -1.21945 -0.283685; 10.5855 -1.1564 -0.281123; 10.583 -1.04575 -0.28233; 10.6477 -1.01788 -0.23278; 10.615 -0.951652 -0.275288; 10.6816 -0.924112 -0.266073; 10.6355 -0.856011 -0.297032; 10.7016 -0.856814 -0.253401; 10.7616 -0.903999 -0.226513; 10.7053 -0.732814 -0.26972; 10.7473 -0.663042 -0.29337; 10.8108 -0.669855 -0.250981; 10.8048 -0.547728 -0.253146; 10.8311 -0.476259 -0.281828; 10.9266 -0.456382 -0.243354; 11.0306 -0.426875 -0.267966; 10.9647 -0.319041 -0.269109; 11.0184 -0.319736 -0.212315; 10.9874 -0.135382 -0.24851; 11.0481 -0.0715804 -0.244176; 11.1979 -0.0276254 -0.295102; 11.1303 0.039961 -0.251273; 11.1996 0.0191852 -0.264362; 11.162 0.110479 -0.21951; 11.2058 0.12348 -0.25323; 11.1934 0.23 -0.28611; 9.3308 -3.90977 -0.250184; 9.38165 -3.81792-0.245416; 9.45995 -3.80769 -0.21086; 9.38964 -3.72947 -0.279179; 9.47933 -3.68575 -0.235764; 9.50964 -3.56801 -0.243825; 9.46994 -3.51543 -0.264041; 9.59309 -3.48991 -0.226894; 9.52824 -3.41873 -0.266959; 9.61154 -3.40862 -0.224342; 9.62392 -3.29423 -0.243159; 9.66449 -3.30101 -0.219465; 9.69367 -3.23911 -0.220725; 9.73016 -3.11929 -0.252037; 9.79572 -3.12838 -0.20255; 9.79417 -3.03543 -0.220278; 9.72693 -2.95897 -0.250664; 9.7922 -2.92347 -0.227478; 9.81952 -2.82852 -0.216248; 9.86686 -2.789 -0.220222; 9.91103 -2.73562 -0.214347; 9.95115 -2.64561 -0.212745; 10.013 -2.63203 -0.192996; 10.0379 -2.55448 -0.209835; 10.0829 -2.43583 -0.208495; 10.1125 -2.31312 -0.205419; 10.1318 -2.15251 -0.209517; 10.2325 -2.13659 -0.158218; 10.2439 -2.0355 -0.190613; 10.2566 -1.95202 -0.158138; 10.3451 -1.93142 -0.148513; 10.2846 -1.83621 -0.208129; 10.3557 -1.82483 -0.17482; 10.3044 -1.73965 -0.212922; 10.3714 -1.73447 -0.185736; 10.3793 -1.64943 -0.179669; 10.4383 -1.62153 -0.196454; 10.4396 -1.55007 -0.172384; 10.4788 -1.46292 -0.15339; 10.5389 -1.42463 -0.18919; 10.5042 -1.35936 -0.182164; 10.5793 -1.31794 -0.166783; 10.5775 -1.24264 -0.186301; 10.612 -1.14501 -0.18449; 10.6331 -1.05633 -0.178497; 10.6742 -1.0389 -0.173252; 10.6316 -0.99706 -0.190606; 10.7302 -0.928719 -0.16027; 10.7694 -0.926909 -0.159048; 10.7291 -0.855626 -0.165113; 10.7701 -0.883856 -0.146772; 10.7389 -0.751469 -0.179787; 10.7782 -0.743712 -0.144529; 10.8251 -0.671326 -0.171348; 10.8963 -0.662618 -0.129921; 10.858 -0.562518 -0.194905; 10.9002 -0.557792 -0.154199; 10.8613 -0.503177 -0.166753; 10.9294 -0.474295 -0.156344; 11.0316 -0.428774 -0.182046; 11.1202 -0.428244 -0.15285; 11.0192 -0.320327 -0.15443; 11.0327 -0.136206 -0.162612; 11.134 -0.0728184 -0.134712; 11.1829 -0.0282952 -0.148128; 11.2018 0.0329562 -0.15871; 11.1882 0.0998269 -0.155042; 11.2066 0.111841 -0.173878; 11.2126 0.219879 -0.173946; 9.44529 -3.91132 -0.149979; 9.49427 -3.80934 -0.161857; 9.53218 -3.80678 -0.141232; 9.50514 -3.71404 -0.175733; 9.54075 -3.69662 -0.139521; 9.52606 -3.588 -0.135668; 9.59216 -3.597 -0.148517; 9.60725 -3.51988 -0.148004; 9.64134 -3.4725 -0.139183; 9.63431 -3.41995 -0.165428; 9.70485 -3.4053 -0.119701; 9.72142 -3.30674 -0.111939; 9.73026 -3.25172 -0.122651; 9.77992 -3.21616 -0.119816; 9.83293 -3.12628 -0.143176; 9.89459 -3.12218 -0.095807; 9.83158 -3.00493 -0.145281; 9.90262 -3.04102 -0.119394; 9.81341 -2.91813 -0.147709; 9.88295 -2.92742 -0.114411; 9.81194 -2.85515 -0.127436; 9.93258 -2.82036 -0.111794; 9.93841 -2.7682 -0.15533; 9.99637 -2.74241 -0.113612; 10.1157 -2.70891 -0.0896455; 10.0325 -2.59818 -0.160119; 10.1123 -2.6464 -0.105417; 10.1219 -2.55081 -0.0978052; 10.1271 -2.4356 -0.128344; 10.2094 -2.31291 -0.125088; 10.2188 -2.14781 -0.116154; 10.2691 -2.06102 -0.0963642; 10.3287 -2.04065 -0.0965721; 10.2912 -1.96907 -0.1229; 10.3677 -1.956 -0.0767181; 10.3662 -1.83308 -0.0925492; 10.3752 -1.75013-0.0945388; 10.4632 -1.72669 -0.0874461; 10.456 -1.63509 -0.104181; 10.4962 -1.53961 -0.0996119; 10.5018 -1.47597 -0.078597; 10.5747 -1.42086 -0.0950892; 10.5831 -1.35219-0.0725823; 10.6022 -1.24453 -0.0683365; 10.651 -1.23134 -0.1054; 10.635 -1.19089 -0.0959604; 10.6853 -1.13479 -0.0674529; 10.6378 -1.09211 -0.100932; 10.7118 -1.02986 -0.0786482; 10.766 -1.06309 -0.0380907; 10.738 -0.965153 -0.0957822; 10.8 -0.927778 -0.0688831; 10.751 -0.907326 -0.100805; 10.8081 -0.876406 -0.0643759; 10.8235 -0.753191 -0.0619738; 10.8495 -0.673867 -0.0906926; 10.9259 -0.668588 -0.0639289; 10.866 -0.581508 -0.105038; 10.9311 -0.55934 -0.0478941; 10.9528 -0.506196 -0.0782152; 11.0237 -0.457769 -0.0468696; 11.1558 -0.429165 -0.0652461; 11.0548 -0.321289 -0.0676317; 11.0423 -0.137271 -0.0759134; 11.178 -0.0709777 -0.0655774; 11.1934 0.0187883 -0.0588069; 11.2262 0.12638 -0.0332021; 11.2304 0.202697 -0.0447574; 9.48841 -3.91198 -0.0484101; 9.4807 -3.8433 -0.0481969; 9.5873 -3.81078 -0.0605782; 9.60544 -3.71672 -0.0473572; 9.59668 -3.57965 -0.0759092; 9.67834 -3.62126 -0.0316276; 9.62915 -3.50785 -0.0693338; 9.6949 -3.50902 -0.0384363; 9.71442 -3.43416 -0.0600424; 9.78346 -3.4023 -0.0300033; 9.71604 -3.3172 -0.0765045; 9.79721 -3.3057 -0.0270053; 9.81187 -3.26424 -0.0223129; 9.8761 -3.21914 -0.0301949; 9.9068 -3.12813 -0.0245095; 9.92994 -3.04014 -0.0403276; 9.97864 -3.03018 0.00583927; 9.9157 -2.93955 -0.024115; 9.92056 -2.86067 -0.0260255; 9.9984 -2.78976 -0.0355203; 10.0432 -2.76855 -0.0222038; 10.1149 -2.7276 -0.0297619; 10.1388 -2.62621 -0.0173701; 10.1388 -2.55025 -0.00815439; 10.2232 -2.43526 -0.0200658; 10.2531 -2.31285 -0.0436093; 10.3052 -2.31275 0.0113838; 10.3237 -2.14667 -0.00688503; 10.3454 -2.04674 -0.0136621; 10.4118 -2.0059 0.00715203; 10.3783 -1.97404 -0.00128607; 10.4501 -1.93079 -0.0108286; 10.346 -1.85441 -0.0306203; 10.4427 -1.83799 0.0181193; 10.4556 -1.73382 0.0015301; 10.4941 -1.64648 -0.00620359; 10.5084 -1.60691 0.02967; 10.5653 -1.54766 0.00370405; 10.5766 -1.44651 -0.000262131; 10.597 -1.31796 -0.0268298; 10.6923 -1.37427 0.0262318; 10.6156 -1.25734 0.0053355; 10.6844 -1.31604 0.0301884; 10.7556 -1.2539 0.0632488; 10.6341 -1.21275 0.0187258; 10.7053 -1.1606 0.0152856; 10.7249 -1.1006 0.009931; 10.774 -1.06858 0.0462306; 10.8107 -0.96275 0.0217242; 10.8409 -0.888021 0.0127966; 10.9055 -0.897708 0.0329045; 10.973 -0.902488 0.0602761; 10.857 -0.755483 0.00726615; 10.9371 -0.774282 0.0349608; 10.9618 -0.662474 0.0116281; 11.0089 -0.692082 0.032674; 10.9012 -0.582051 0.00945054; 10.9902 -0.56359 0.0483869; 10.9653-0.499247 0.0229407; 11.0517 -0.482807 0.057561; 11.1556 -0.429956 0.0226927; 11.0899 -0.322254 0.0196285; 11.1421 -0.322966 0.0784549; 11.19 -0.0870306 0.00676893; 11.2061 -0.098813 0.0513141; 11.2254 0.111134 0.0145849; 9.51349 -3.91239 0.0282982; 9.65285 -3.91419 0.0828884; 9.60679 -3.81277 0.0252326; 9.64593 -3.82429 0.0450638; 9.66954-3.71873 0.056857; 9.68726 -3.61448 0.0433194; 9.76677 -3.66415 0.0918766; 9.69802 -3.54462 0.0468132; 9.7778 -3.51429 0.0816422; 9.8091 -3.4251 0.0539615; 9.81904 -3.32762 0.0587491; 9.88576 -3.30903 0.0674805; 9.84083 -3.26827 0.0618625; 9.89304 -3.23162 0.0623171; 9.96025 -3.15558 0.0984303; 9.96784 -3.14473 0.0539837; 9.95104 -3.02581 0.0779228; 9.99709 -3.06066 0.0720247; 9.92063 -2.94471 0.0492693; 9.98636 -2.9397 0.0893263; 9.9512 -2.85651 0.0311476; 10.0091 -2.8451 0.0670005; 10.1471 -2.74937 0.0863036; 10.1513 -2.65379 0.0640796; 10.2038 -2.6337 0.0899845; 10.1878 -2.59667 0.109622; 10.2244 -2.56193 0.070482; 10.2963 -2.54381 0.0773766; 10.3182 -2.43492 0.0901331; 10.3302 -2.31275 0.093862; 10.3678 -2.14225 0.0891059; 10.3908 -2.04601 0.0875503; 10.4586 -2.0845 0.115836; 10.3941 -1.98543 0.0777159; 10.4473 -1.95897 0.104954; 10.4671 -1.85374 0.108656; 10.487 -1.7611 0.107101; 10.5408 -1.72598 0.0798703; 10.5246 -1.65581 0.082447; 10.5587 -1.65522 0.138912; 10.5954 -1.5778 0.114642; 10.6138 -1.48699 0.127996; 10.6976 -1.45839 0.109139; 10.706 -1.36312 0.100601; 10.768 -1.37315 0.146504; 10.7672 -1.28407 0.118452; 10.7742 -1.17606 0.102722; 10.7586 -1.10074 0.123954; 10.783 -1.0378 0.110819; 10.823 -1.00743 0.122387; 10.9285 -0.989704 0.153037; 10.8165 -0.916561 0.0978409; 10.9234 -0.901174 0.142611; 10.9794 -0.880056 0.105747; 10.9571 -0.771813 0.136678; 10.9893 -0.784238 0.118435; 10.9755 -0.705777 0.105339; 11.023 -0.671918 0.130651; 10.9703 -0.582615 0.125216; 11.0558 -0.554695 0.126333; 11.0477 -0.500206 0.124951; 11.1532 -0.451544 0.13252; 11.1899 -0.432096 0.094811; 11.1765 -0.323643 0.137548; 11.1819 -0.139031 0.128713; 9.66803 -3.91448 0.160904; 9.68167 -3.83275 0.139195; 9.71935 -3.73914 0.146122; 9.75329 -3.74039 0.186591; 9.73009 -3.66445 0.14291; 9.79516 -3.61631 0.167757; 9.81156 -3.53439 0.165696; 9.85973 -3.53504 0.174083; 9.88514 -3.43661 0.16436; 9.91968 -3.33778 0.15187; 9.9324 -3.26364 0.171076; 9.96587 -3.26228 0.148313; 10.0064 -3.15503 0.16343; 10.0269 -3.04411 0.158226; 10.122 -3.05528 0.196648; 10.0333 -2.937 0.183527; 9.99945 -2.87763 0.156472; 10.1361 -2.85202 0.18725; 10.1614 -2.77304 0.189261; 10.1994 -2.72079 0.149458; 10.2306 -2.64484 0.17535; 10.2607 -2.56801 0.182002; 10.3104 -2.56147 0.154165; 10.3777 -2.43467 0.173807; 10.3896 -2.31265 0.177618; 10.3972 -2.14935 0.191564; 10.4218 -2.13394 0.171837; 10.4573 -2.06454 0.19972; 10.4973 -1.97092 0.197013; 10.5502 -1.93074 0.241106; 10.5263 -1.86399 0.186685; 10.5521 -1.85337 0.222047; 10.5292 -1.77798 0.177711; 10.5679 -1.75734 0.219026; 10.5983 -1.67236 0.21163; 10.6198 -1.59904 0.193113; 10.6632 -1.5628 0.22872; 10.6831 -1.47237 0.203924; 10.7422 -1.36268 0.21491; 10.7929 -1.36314 0.209731; 10.7433-1.285 0.203277; 10.8224 -1.27723 0.234101; 10.7542 -1.21055 0.190429; 10.8028 -1.18923 0.221532; 10.8163 -1.07892 0.213447; 10.8218 -0.993087 0.208063; 10.9079 -0.990788 0.268045; 10.9489 -0.917558 0.202053; 11.0063 -0.889393 0.208372; 11.0199 -0.792608 0.226216; 11.0435 -0.692776 0.228854; 11.1049 -0.648901 0.226356; 11.0502 -0.582899 0.228836; 11.0551 -0.508939 0.231282; 11.1186 -0.495549 0.217472; 11.1351 -0.433019 0.211238; 11.1754 -0.324239 0.196255; 9.71779 -3.9152 0.240655; 9.7209 -3.82929 0.219661;9.78537 -3.83818 0.262427; 9.73293 -3.77838 0.232228; 9.81154 -3.74175 0.267178; 9.8335 -3.59283 0.259291; 9.84913 -3.6665 0.252067; 9.83307 -3.54771 0.253246; 9.90276 -3.51235 0.250016; 9.91563 -3.45316 0.258771; 9.99503 -3.40504 0.259323; 9.93484 -3.33578 0.233572; 9.99505 -3.3316 0.274067; 9.95062 -3.26329 0.236381; 10.0159 -3.2629 0.284236; 10.0294 -3.1597 0.245116; 10.0868 -3.13792 0.295504; 10.0456 -3.03477 0.22686; 10.1144 -3.06073 0.268022; 10.0199 -2.96398 0.225421; 10.1615 -2.95756 0.279155; 10.1498 -2.85099 0.268252; 10.1892 -2.85781 0.288387; 10.2081 -2.76092 0.269119; 10.3165 -2.74657 0.307328; 10.2511 -2.65509 0.252428; 10.3256 -2.65762 0.287955; 10.3313 -2.56044 0.280142; 10.3759 -2.43454 0.228726; 10.4441 -2.43435 0.286411; 10.4396 -2.31258 0.262067; 10.4762 -2.14849 0.28635; 10.5027 -2.03824 0.282851; 10.5412 -2.0832 0.285123; 10.5054 -1.97983 0.291078; 10.5474 -1.95489 0.295666; 10.5529 -1.85034 0.289534; 10.5918 -1.76733 0.311378; 10.6131 -1.7146 0.322472; 10.6616 -1.66564 0.28751; 10.6654 -1.58331 0.285353; 10.7183 -1.49173 0.298007; 10.7451 -1.41171 0.267141; 10.7864 -1.39221 0.314237; 10.8057 -1.30131 0.316984; 10.8379 -1.21368 0.315087; 10.8438 -1.11419 0.321609; 10.9106 -1.08875 0.359816; 10.9057 -1.02822 0.304154; 10.924 -0.915753 0.291507; 11.0332 -0.897894 0.312195; 11.0282 -0.786576 0.322606; 11.0661 -0.690616 0.318515; 11.1156 -0.680957 0.356167; 11.0644 -0.592654 0.322735; 11.1034 -0.638662 0.363944; 11.0682 -0.497561 0.287967; 11.1369 -0.502114 0.341839; 11.1878 -0.326066 0.372982; 9.73302 -3.91546 0.293436; 9.78308 -3.91615 0.348235; 9.80943 -3.8368 0.352877; 9.85973 -3.83664 0.35808; 9.80248 -3.7047 0.299118; 9.86931 -3.74361 0.349098; 9.90687 -3.66781 0.334066; 9.91818 -3.54688 0.346471; 9.94704 -3.46704 0.373741; 10.0028 -3.434 0.346304; 10.005 -3.34213 0.352791; 10.0382 -3.27585 0.356124; 10.0828 -3.25537 0.376386; 10.1032 -3.16611 0.347883; 10.1795 -3.1294 0.40056; 10.128 -3.05941 0.357694; 10.1405 -2.97174 0.361335; 10.1922 -2.91637 0.35571; 10.1809 -2.85829 0.347943; 10.2016 -2.8695 0.397045; 10.1883 -2.79087 0.372264; 10.2761 -2.74601 0.414852; 10.2965 -2.75781 0.348615; 10.3559 -2.65977 0.366561; 10.3546 -2.59531 0.360912; 10.4143 -2.54802 0.38176; 10.4317 -2.43416 0.368865; 10.4538 -2.31261 0.345755; 10.5214 -2.31247 0.404601; 10.4862 -2.13286 0.340183; 10.5416 -2.14768 0.400602; 10.5053 -2.04426 0.353585; 10.5492 -2.07604 0.379814; 10.5231 -2.01491 0.381613; 10.5849 -1.97031 0.386895; 10.6127 -1.87566 0.402688; 10.6437 -1.84272 0.372576; 10.6353 -1.77178 0.400931; 10.6544 -1.7481 0.384542; 10.6928 -1.67313 0.401282; 10.7137 -1.59719 0.394169; 10.7374 -1.50487 0.380037; 10.7933 -1.50374 0.439698; 10.7493 -1.42209 0.377772; 10.8257 -1.41082 0.443243; 10.8635 -1.39027 0.397191; 10.818 -1.31861 0.396416; 10.8836 -1.3338 0.429602; 10.834 -1.20946 0.417157; 10.8718 -1.19812 0.364883; 10.8721 -1.11014 0.41144; 10.8883 -1.12984 0.41824; 10.9401 -1.03093 0.413989; 11.0638 -0.909606 0.436634; 11.0976 -0.880623 0.45409; 11.0761 -0.799109 0.422237; 11.0689 -0.697764 0.393634; 11.1156 -0.696144 0.435628; 11.1165 -0.619945 0.422897; 11.1322 -0.522911 0.430493; 11.1801 -0.435214 0.447526; 11.1855 -0.32666 0.431745; 9.79616 -3.91641 0.427769; 9.83199 -3.85181 0.430348; 9.86996 -3.84123 0.456604; 9.84542 -3.76361 0.450479; 9.87025 -3.73509 0.416817; 9.93606 -3.66921 0.441923; 9.93061 -3.58253 0.443103; 9.98175 -3.54441 0.447138; 9.94631 -3.46176 0.417899; 10.0182 -3.45999 0.455494; 10.0488 -3.34919 0.454558; 10.0362 -3.28206 0.43299; 10.1205 -3.27288 0.468152; 10.1441 -3.15635 0.456655; 10.1887 -3.19187 0.47578; 10.1635 -3.06194 0.433491; 10.1965 -3.05996 0.472011; 10.1773 -2.97744 0.468458; 10.215 -2.92769 0.473557; 10.2267 -2.85882 0.456631; 10.2805 -2.7914 0.458916; 10.3347 -2.75775 0.46032; 10.3826 -2.65415 0.450427; 10.4328 -2.66692 0.493102; 10.4039 -2.56814 0.456083; 10.4364 -2.5915 0.473339; 10.4873 -2.43384 0.482873; 10.501 -2.31258 0.459333; 10.5328 -2.31254 0.517041; 10.5159 -2.13226 0.452947; 10.5575 -2.15513 0.486451; 10.5311 -2.06222 0.511668; 10.5677 -2.09203 0.474586; 10.5917 -1.9884 0.492449; 10.657 -2.00313 0.517884; 10.6378 -1.8644 0.471493; 10.6973 -1.89178 0.505402; 10.688 -1.77747 0.493111; 10.7185 -1.69226 0.496471; 10.7212 -1.5971 0.508268; 10.7914 -1.59564 0.512104; 10.7991 -1.50533 0.510979; 10.8395 -1.41573 0.513727; 10.8535 -1.32411 0.503119; 10.8844 -1.30535 0.520137; 10.866 -1.22878 0.469009; 10.9137 -1.20395 0.514802; 10.9379 -1.10924 0.504909; 10.9594 -1.03543 0.490937; 11.0738 -0.986877 0.507865; 11.0782 -0.912256 0.51058; 11.1028 -0.890798 0.5047; 11.078 -0.802291 0.526132; 11.1043 -0.815446 0.518198; 11.1262 -0.708642 0.524045; 11.1498 -0.627262 0.530239; 11.1568 -0.522553 0.564914; 9.8857 -3.91767 0.539336; 9.918 -3.83516 0.528951; 9.92025 -3.7561 0.523226; 9.96135 -3.74716 0.567217; 9.98682 -3.6838 0.559923; 9.99553 -3.56604 0.54869; 10.0376 -3.47644 0.539717; 10.0993 -3.42678 0.588635; 10.0681 -3.40676 0.531337; 10.1144 -3.35938 0.550062; 10.1289 -3.27175 0.54031; 10.1866 -3.27168 0.596725; 10.1488 -3.18566 0.553019; 10.2122 -3.15215 0.55368; 10.2531 -3.04081 0.572746; 10.2541 -2.97384 0.576415; 10.2726 -2.85921 0.537873; 10.335 -2.79172 0.572114; 10.4064 -2.74614 0.587985; 10.4339 -2.67127 0.5593; 10.442 -2.57871 0.573303; 10.5173 -2.43362 0.568364; 10.5644 -2.3125 0.5751; 10.5787 -2.1548 0.571851; 10.6264 -2.0912 0.571866; 10.6549 -2.07088 0.630401; 10.635 -2.0134 0.584222; 10.6667 -1.97101 0.606481; 10.6955 -1.87047 0.580143; 10.7138 -1.77762 0.579697; 10.7538 -1.80683 0.633011; 10.7238 -1.72466 0.565654; 10.7772 -1.70365 0.615156; 10.7526 -1.64048 0.549217; 10.7739 -1.61074 0.600507; 10.8069 -1.51061 0.599542; 10.8697 -1.46935 0.555134; 10.8527 -1.41098 0.589231; 10.8796 -1.40991 0.588782; 10.8566 -1.27777 0.637785; 10.8828 -1.32373 0.605759; 10.9301 -1.20799 0.595378; 10.9663 -1.10956 0.622638; 11.0137 -1.02718 0.619212; 11.0524 -0.902952 0.594047; 11.0982 -0.922013 0.577529; 11.1195 -0.810183 0.610089; 11.1232 -0.7299 0.604163; 11.1441 -0.612432 0.628357; 11.1607 -0.509843 0.592501; 9.9302 -3.91836 0.649204; 9.92634 -3.84303 0.62133; 9.98603 -3.8277 0.66511; 9.95522 -3.7482 0.647049; 9.99856 -3.78754 0.645254; 9.99754 -3.69118 0.631726; 10.0312 -3.57309 0.653733; 10.0507 -3.49701 0.641707; 10.1165 -3.45075 0.652888; 10.1396 -3.36932 0.639894; 10.1712 -3.29571 0.642438; 10.1908 -3.27393 0.640753; 10.2395 -3.17417 0.685222; 10.3369 -3.13377 0.684834; 10.2813 -3.07195 0.647207; 10.3315 -3.07211 0.678705; 10.2708 -2.95921 0.637451; 10.3098 -2.99913 0.677141; 10.3189 -2.87603 0.661379; 10.3835 -2.76911 0.647031; 10.4332 -2.74592 0.701103; 10.4435 -2.67062 0.671229; 10.4771 -2.56847 0.674374; 10.5553 -2.43338 0.655072; 10.5761 -2.31254 0.660306; 10.5885 -2.15451 0.685175; 10.5978 -2.10621 0.681661; 10.7125 -2.05986 0.693246; 10.6832 -1.98434 0.674546; 10.7126 -1.89776 0.673409; 10.7413 -1.81072 0.678643; 10.7414 -1.74223 0.69706; 10.8047 -1.68758 0.691801; 10.8089 -1.61006 0.688935; 10.8507 -1.52304 0.695302; 10.8799 -1.48266 0.710313; 10.9066 -1.41008 0.707778; 10.8529 -1.33526 0.658506; 10.9034 -1.31452 0.701629; 11.0237 -1.31012 0.734844; 10.9792 -1.18703 0.659261; 11.0277 -1.19631 0.722101; 11.0307 -1.10857 0.714533; 11.0372 -1.03476 0.70967; 11.1065 -0.878562 0.757947; 11.1445 -0.706425 0.744244; 11.1525 -0.585691 0.774703; 9.9672 -3.91893 0.732675; 10.0058 -3.84319 0.740288; 10.0534 -3.76995 0.733382; 10.0565 -3.6927 0.71748; 10.0673 -3.52553 0.697399; 10.1021 -3.56827 0.759493; 10.1208 -3.47282 0.746045; 10.1651 -3.40175 0.727922; 10.2225 -3.36075 0.782113; 10.2338 -3.28893 0.750194; 10.263 -3.18184 0.751411; 10.3289 -3.15529 0.754992; 10.3373 -3.09225 0.756034; 10.292 -2.95065 0.71407; 10.3483 -2.99303 0.764737; 10.354 -2.87542 0.765045; 10.403 -2.76885 0.734865; 10.4325 -2.77702 0.763897; 10.4754 -2.69222 0.775131; 10.4829 -2.60886 0.764006; 10.5584 -2.53958 0.783934; 10.5903 -2.43307 0.770399; 10.5871 -2.31259 0.745653; 10.67 -2.3124 0.809279; 10.6608 -2.15374 0.775676; 10.6023 -2.13062 0.739177; 10.6691 -2.08267 0.787851; 10.698 -2.00646 0.782607; 10.7764 -1.98185 0.827634; 10.7361 -1.91498 0.785233; 10.7427 -1.82011 0.775389; 10.7718 -1.84432 0.835852; 10.7587 -1.74874 0.817471; 10.8176 -1.70921 0.786165; 10.8335 -1.60738 0.792264; 10.9104 -1.58761 0.83094; 10.8996 -1.50232 0.792256; 10.9251 -1.41501 0.808763; 10.9392 -1.32053 0.807244; 11.0104-1.29253 0.807802; 10.9794 -1.20804 0.812284; 11.0362 -1.1868 0.83908; 10.9852 -1.13088 0.830795; 11.0394 -1.10931 0.829009; 11.1162 -0.878449 0.774739; 11.1261 -0.802965 0.800647; 11.1328 -0.74462 0.827849; 11.1464 -0.661763 0.793066; 11.1599 -0.531597 0.821363; 10.0166 -3.86337 0.84095; 10.0547 -3.77111 0.814749; 10.0669 -3.71342 0.797971; 10.1099 -3.68801 0.840645; 10.0623 -3.57998 0.798873; 10.1372 -3.57975 0.843299; 10.1603 -3.50266 0.837278; 10.1994 -3.47838 0.834328; 10.2008 -3.39952 0.839468; 10.2608 -3.28984 0.846316; 10.3034 -3.27479 0.881468; 10.3163 -3.17646 0.848083; 10.3406 -3.08311 0.849949; 10.3421 -2.98572 0.864247; 10.3929 -2.92013 0.864731; 10.431 -2.87606 0.866688; 10.3967 -2.83945 0.869768; 10.4421 -2.76904 0.85698; 10.4861 -2.6783 0.859959; 10.5168 -2.59295 0.853585; 10.5439 -2.60558 0.900373; 10.6035 -2.53906 0.872657; 10.6126 -2.43272 0.913822; 10.6704 -2.43279 0.86249; 10.6421 -2.31259 0.920493; 10.6649 -2.31247 0.86575; 10.6735 -2.15353 0.833611; 10.7037 -2.08964 0.861557; 10.7346 -2.00207 0.8889; 10.7809 -1.92328 0.887315; 10.7971 -1.82017 0.894766; 10.8335 -1.71769 0.887207; 10.8668 -1.6385 0.902884; 10.8995 -1.60835 0.879078; 10.9273 -1.50194 0.881827; 10.9477 -1.41627 0.891535; 10.9401 -1.33392 0.897203; 11.0181 -1.29275 0.925815; 11.0138 -1.20764 0.893208; 11.0267 -1.12053 0.91918; 11.1144 -0.832105 0.877437; 11.1265 -0.758472 0.880246; 10.042 -3.89054 0.914752; 10.0753 -3.85049 0.936824; 10.1142 -3.78549 0.931887; 10.1405 -3.69604 0.943452; 10.145 -3.58404 0.938033; 10.1736 -3.48655 0.94455; 10.1924 -3.51335 0.919352; 10.2239 -3.40964 0.943935; 10.2515 -3.30437 0.94097; 10.3097 -3.28283 0.930268; 10.3408 -3.17266 0.944845; 10.3659 -3.09442 0.94197; 10.3921 -2.98001 0.954957; 10.4263 -2.9714 0.96367; 10.3704 -2.88838 0.936427; 10.4384 -2.86252 0.984644; 10.4087 -2.83971 0.926681; 10.4807 -2.79869 0.974427; 10.5006 -2.7049 0.942127; 10.5222 -2.63843 0.923034; 10.5551 -2.60445 0.961686; 10.5986 -2.54439 0.974408; 10.6594 -2.43253 0.975347; 10.654 -2.31261 0.978609; 10.7187 -2.14091 0.964473; 10.7853 -2.01087 0.969032; 10.8132 -1.91883 0.965095; 10.817 -1.85525 1.00594; 10.8834 -1.82559 0.975404; 10.8305 -1.74853 0.996682; 10.893 -1.72647 0.984144; 10.8753-1.59987 1.01525; 10.9112 -1.62452 0.979989; 10.913 -1.51897 0.998739; 10.9551 -1.43303 0.990137; 10.0732 -3.92067 1.06867; 10.1287 -3.87295 1.02103; 10.1353 -3.80501 1.03535; 10.1556 -3.70405 1.04516; 10.168 -3.59199 1.04699; 10.2038 -3.58522 1.04534; 10.1701 -3.50712 1.01756; 10.2216 -3.47989 1.05194; 10.2921 -3.30722 1.01744; 10.3239 -3.27599 1.02243; 10.3521 -3.18548 1.03716; 10.3812 -3.10989 1.04331; 10.4348 -3.071 1.07526; 10.4078 -2.98998 1.04811; 10.449 -2.9843 1.07351; 10.4136 -2.92089 1.00631; 10.4674 -2.88307 1.05684; 10.5047 -2.81087 1.07263; 10.5585 -2.70454 1.06126; 10.6002 -2.62053 1.06287; 10.6177 -2.54379 1.07118; 10.7171 -2.4322 1.096; 10.7061 -2.31254 1.07004; 10.73 -2.12826 1.06572; 10.7792 -2.07866 1.09705; 10.7636 -2.02063 1.12004; 10.8073 -1.99726 1.07876; 10.8299 -1.93981 1.07902; 10.8358 -1.83999 1.08695; 10.8688 -1.73306 1.08921; 10.8945 -1.74781 1.06127; 10.9222 -1.6189 1.09717; 10.9557 -1.52847 1.09502; 11.0094 -1.38989 1.04789; 11.0357 -1.33233 1.12172; 10.1329 -3.88165 1.11293; 10.1853 -3.86292 1.1658; 10.1638 -3.81261 1.13034; 10.171 -3.71869 1.13769; 10.1999 -3.67852 1.11704; 10.1953 -3.60276 1.17935; 10.2074 -3.58248 1.12571; 10.2452 -3.50027 1.13238; 10.2653 -3.4307 1.13302; 10.3069 -3.40131 1.16991; 10.2826 -3.33385 1.1713; 10.3442 -3.30612 1.15459; 10.3809 -3.18495 1.13218; 10.4291 -3.1705 1.14238; 10.4168 -3.06517 1.15401; 10.4447 -3.09525 1.15161; 10.4346 -3.00311 1.16293; 10.4492 -2.9164 1.14477; 10.4887 -2.84095 1.16013; 10.5665 -2.78126 1.16017; 10.5804 -2.71454 1.1381; 10.6115 -2.61931 1.16604; 10.6328 -2.55521 1.15508; 10.6784 -2.53728 1.16568; 10.7279 -2.31258 1.18745; 10.7258 -2.15244 1.18239; 10.7997 -2.11111 1.17746; 10.8106 -2.01987 1.18695; 10.8198 -1.94312 1.18215; 10.862 -1.83757 1.16962; 10.9021 -1.82703 1.21044; 10.8469 -1.74923 1.1723; 10.9259 -1.7197 1.19531; 10.9359 -1.63353 1.18184; 10.9493 -1.52875 1.15293; 11.0315 -1.50666 1.21279; 11.0158 -1.42905 1.14788; 11.0329 -1.33265 1.20014; 10.1605 -3.92195 1.21715; 10.2129 -3.8638 1.22488; 10.181 -3.75804 1.18766; 10.1982 -3.81146 1.21809; 10.2559 -3.70043 1.23365; 10.2702 -3.58389 1.23728; 10.2824 -3.51185 1.23476; 10.3007 -3.43116 1.21301; 10.3339 -3.41981 1.25092; 10.3693 -3.30127 1.2459; 10.4007 -3.2008 1.21455; 10.4383 -3.20097 1.24936; 10.4614 -3.10612 1.23857; 10.4261 -3.01878 1.24726; 10.4635 -2.97333 1.24928; 10.5138 -2.90988 1.25985; 10.5119 -2.84888 1.25704; 10.6006 -2.79329 1.27172; 10.623 -2.73347 1.25236; 10.6247 -2.64747 1.26377; 10.6563 -2.62094 1.25333; 10.6825 -2.5456 1.2642; 10.7386 -2.4319 1.21365;10.7662 -2.43173 1.27495; 10.7434 -2.31262 1.27595; 10.7937 -2.1272 1.27527; 10.8231 -2.01578 1.26762; 10.8378 -1.94884 1.27741; 10.8701 -1.8664 1.23714; 10.8972 -1.84606 1.31292; 10.9272 -1.73612 1.2735; 10.9606 -1.62802 1.25198; 10.9969 -1.63624 1.32683; 11.0016 -1.58592 1.30911; 10.1841 -3.92235 1.30348; 10.2049 -3.879 1.31389; 10.235 -3.81004 1.32502; 10.271 -3.70858 1.33758; 10.2862 -3.58494 1.35128; 10.3025 -3.60433 1.30513; 10.3181 -3.50649 1.33892; 10.3019 -3.45204 1.30181; 10.3716 -3.41922 1.33514;10.3959 -3.30704 1.32635; 10.4239 -3.27489 1.34961; 10.4756 -3.19259 1.32083; 10.4898 -3.12481 1.34343; 10.5 -3.00348 1.33494; 10.5513 -3.01151 1.37991; 10.5111 -2.92316 1.35699; 10.5829 -2.84113 1.35967; 10.6163 -2.74632 1.35209; 10.631 -2.64907 1.3464; 10.6647 -2.59832 1.33332; 10.6929 -2.54197 1.3673; 10.7239 -2.43164 1.3272; 10.7865 -2.1272 1.33081; 10.9046 -2.0317 1.39332; 10.8858 -1.98103 1.41992; 10.9325 -1.83295 1.38678; 10.9732 -1.73591 1.39714; 11.0033 -1.71113 1.3342; 10.9685 -1.6605 1.42542; 11.0089 -1.64786 1.37963; 11.0168 -1.55164 1.39638; 10.2236 -3.8773 1.42129; 10.2597 -3.83352 1.43068; 10.3086 -3.71287 1.42153; 10.3033 -3.59594 1.42644; 10.3291 -3.59063 1.45926; 10.3044 -3.5222 1.40305; 10.3468 -3.50426 1.44452; 10.3837 -3.40797 1.44755; 10.3991 -3.3136 1.41491; 10.4538 -3.28758 1.45147; 10.4688 -3.2179 1.44169; 10.5051 -3.11697 1.45069; 10.54 -3.0826 1.4221; 10.5226 -3.02109 1.4434; 10.5703 -3.06885 1.45898; 10.5084 -2.93943 1.41187; 10.5639 -2.92558 1.46736; 10.5812 -2.82599 1.44221; 10.6991 -2.62524 1.4669; 10.7564 -2.55314 1.48899; 10.7801 -2.53546 1.46838; 10.8273 -2.15104 1.486; 10.8841 -2.12577 1.51986; 10.8702 -2.03351 1.47435; 10.8935 -2.01817 1.48592; 10.895 -1.98097 1.47994; 10.9532 -1.83342 1.4928; 10.9677 -1.73311 1.49748; 11.0051 -1.71147 1.45284; 10.9944 -1.63262 1.48414; 11.0188 -1.59714 1.44341; 11.0056 -1.04188 1.55165; 10.2506 -3.89524 1.50938; 10.2719 -3.839 1.52119; 10.2897 -3.73748 1.50555; 10.3439 -3.71096 1.53534; 10.3443 -3.58672 1.52906; 10.3603 -3.51126 1.52094; 10.4193 -3.53364 1.57252; 10.3934 -3.44702 1.52999; 10.4318 -3.41369 1.53375; 10.4178 -3.33596 1.58513; 10.4481 -3.31094 1.53628; 10.4763 -3.2316 1.53238; 10.5098 -3.12416 1.54095; 10.541 -3.08119 1.5294; 10.5516 -3.03768 1.55693; 10.5717 -2.9302 1.56167; 10.6043 -2.84195 1.55485; 10.6297 -2.74414 1.57455; 10.6838 -2.73886 1.53111; 10.7028 -2.63685 1.56462; 10.7251 -2.56303 1.57815; 10.7716 -2.53508 1.52546; 10.8387 -2.43109 1.51816; 10.8377 -2.31259 1.55141; 10.8405 -2.1425 1.57462; 10.8451 -2.04769 1.5544; 10.8429 -1.98157 1.58953; 10.9038 -1.98091 1.54011; 10.9237 -1.84859 1.56654; 10.9649 -1.73202 1.58369; 10.9794 -1.65257 1.59026; 10.9957 -1.55529 1.6046; 10.997 -1.04256 1.60983; 11.0222 -0.851524 1.61365; 10.2583 -3.92368 1.62287; 10.2914 -3.84587 1.60181; 10.3387 -3.83208 1.64823; 10.3475 -3.73405 1.6323; 10.3821 -3.59391 1.6336; 10.3872 -3.51939 1.64246; 10.412 -3.43404 1.65288; 10.4454 -3.41437 1.63658; 10.4794 -3.312 1.63879; 10.4905 -3.25091 1.62669; 10.5446 -3.1866 1.62896; 10.5326 -3.16714 1.65511; 10.5622 -3.10521 1.60463; 10.5736 -3.0525 1.65245; 10.5726 -2.94434 1.64473; 10.6087 -2.84228 1.65943; 10.6324 -2.73957 1.66626; 10.6669 -2.73558 1.61846; 10.6445 -2.64705 1.66578; 10.6756 -2.6241 1.64379; 10.668 -2.55163 1.70768; 10.7168 -2.55183 1.66363; 10.8329 -2.31269 1.63878; 10.8489 -2.12572 1.63205; 10.8584 -2.03778 1.63985; 10.8819 -1.8488 1.66334; 10.9058 -1.73724 1.63951; 10.9632 -1.74561 1.69458; 10.9817 -1.65264 1.68973; 10.9855 -1.59217 1.66948; 10.9913 -1.21341 1.6628; 10.3296 -3.85009 1.72005; 10.3757 -3.71696 1.71613; 10.3761 -3.60224 1.73782; 10.41 -3.52186 1.73552; 10.4381 -3.51126 1.74268; 10.4166 -3.43479 1.75046; 10.4483 -3.41475 1.72303; 10.4677 -3.31442 1.73566; 10.5031 -3.24943 1.73434; 10.5418 -3.20659 1.69156; 10.5212 -3.14397 1.74927; 10.5384 -3.06727 1.76819; 10.5694 -3.04653 1.73747; 10.5778 -2.95331 1.74357; 10.5938 -2.84491 1.74821; 10.6187 -2.73659 1.76521; 10.6474 -2.63489 1.75433; 10.6816 -2.61376 1.71757; 10.6582 -2.55125 1.76417; 10.7037 -2.53341 1.77714; 10.7346 -2.31313 1.79829; 10.7801 -1.9561 1.80466; 10.8411 -1.84193 1.75202; 10.8596 -1.65107 1.83025; 10.8655 -1.55107 1.81893; 10.9665 -1.59262 1.78555; 10.909 -1.4147 1.79048; 10.9235 -1.33729 1.83635; 10.947 -1.14134 1.82789; 10.9628 -0.909601 1.8127; 10.9412 -0.897216 1.85794; 10.9784 -0.832982 1.79575; 10.3422 -3.84371 1.83159; 10.3594 -3.73637 1.80554; 10.3907 -3.59666 1.82286; 10.4033 -3.5488 1.81554; 10.4349 -3.51202 1.85693; 10.4179 -3.43507 1.80805; 10.4501 -3.41512 1.84424; 10.4309 -3.3368 1.81634; 10.4728 -3.31519 1.84988; 10.4941 -3.25734 1.83477; 10.5059 -3.18202 1.83345; 10.5557 -3.14875 1.84477; 10.5345 -3.07175 1.84836; 10.5647 -3.01438 1.84275; 10.5488 -2.95921 1.84898; 10.5835 -2.92599 1.88663; 10.5565 -2.87683 1.86587; 10.5826 -2.84767 1.82566; 10.5803 -2.78083 1.85119; 10.6501 -2.54168 1.86719; 10.6846 -2.43031 1.90207; 10.6793 -2.3134 1.90533;10.707 -2.31327 1.85193; 10.6922 -2.15144 1.87312; 10.7129 -2.12603 1.90199; 10.7334 -2.03861 1.89733; 10.7602 -1.96296 1.89234; 10.7879 -1.85278 1.89776; 10.8039 -1.74107 1.9013; 10.8432 -1.72732 1.85339; 10.8052 -1.65216 1.90917; 10.8386 -1.57066 1.90389; 10.882 -1.33007 1.92665; 10.9267 -1.25809 1.93582; 10.9311 -1.1458 1.91876; 10.9261-1.07565 1.94829; 10.9772 -0.793091 1.88763; 10.3338 -3.92496 1.89261; 10.3503 -3.84874 1.8793; 10.3859 -3.77 1.89776; 10.4089 -3.64974 1.93035; 10.403 -3.54627 1.94311; 10.4249 -3.44083 1.93027; 10.4518 -3.41559 1.91774; 10.4579 -3.31905 1.91564; 10.4711 -3.27067 1.92498; 10.487 -3.16975 1.90654; 10.5332 -3.03746 1.9268; 10.5386 -2.95395 1.93924; 10.5612 -2.86609 1.9508; 10.6427 -2.54112 1.95346; 10.6738 -2.43019 1.95857; 10.6857 -2.31344 1.96518; 10.7022 -2.14278 1.96147; 10.7045 -2.06689 1.98893; 10.7454-2.03751 1.97976; 10.7453 -1.96014 1.99149; 10.7648 -1.84738 1.99142; 10.7903 -1.74154 1.99646; 10.8021 -1.6523 1.99732; 10.8748 -1.339 2.00611; 10.8969 -1.25185 2.03039;10.9087 -1.16239 2.03619; 10.9154 -1.07647 2.00602; 10.9338 -0.919646 2.03306; 10.9718 -0.751987 2.05016; 10.9761 -0.731596 2.03416; 10.9785 -0.657114 2.08164; 10.3742 -3.87372 2.04559; 10.4223 -3.69017 2.03065; 10.4061 -3.56976 2.02612; 10.436 -3.4542 2.01583; 10.4892 -3.28339 2.00282; 10.5379 -2.96045 2.04332; 10.596 -2.92659 2.00575; 10.5581 -2.87745 2.04062; 10.63 -2.53175 2.0266; 10.7117 -2.01895 2.07203; 10.758 -1.8533 2.05439; 10.8575 -1.33438 2.0938; 10.8887 -1.21894 2.09198; 10.8738 -1.2007 2.11695; 10.9057 -1.04908 2.13112; 10.9614 -0.765031 2.09502; 10.4173 -3.56214 2.10166; 10.8876 -1.35114 2.20572; 10.8994 -1.2599 2.16995; 10.8919 -1.16692 2.21074; 10.8921 -1.06507 2.20796; 10.8912 -0.999276 2.19963; 10.431 -3.73466 2.20401; 10.8596 -1.20284 2.35401; 10.8742 -1.18365 2.29397; 10.8794 -1.10899 2.29113] -#################################### -#### TODO stuff -#################################### - - -# type Cloud -# points:: -# colors:: -# end - -# function add_pointcloud!(slaml::SyncrSLAM, nodeID::Symbol, cloud::Cloud ) -# # fetch from database -# vert = getVert(slaml.fg, nodeID, api=IncrementalInference.dlapi) - -# # add points blob -# serialized_point_cloud = BSONObject(Dict("pointcloud" => cloud.points)) -# appendvertbigdata!(slaml.fg, vert, "BSONpointcloud", string(serialized_point_cloud).data) - -# # add colors blob -# serialized_colors = BSONObject(Dict("colors" => cloud.colors)) -# appendvertbigdata!(slaml.fg, vert, "BSONcolors", string(serialized_colors).data) -# end +# Todo - call Sychrony to start the solver From 49c2961a5cf31e8a48098c630587da6c9c0753ed Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 16 May 2018 16:20:46 -0400 Subject: [PATCH 26/27] test change in travis file --- .travis.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index de7abb96c..48bad1f08 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,4 @@ language: julia -dist: trusty -sudo: required os: - linux - osx From 0441048aab05b0ac8898008562ce797b780d99c7 Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 16 May 2018 22:34:18 -0400 Subject: [PATCH 27/27] add package --- .travis.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.travis.yml b/.travis.yml index 48bad1f08..febd54ca5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,6 +7,10 @@ julia: - nightly notifications: email: false +addons: + apt: + packages: + - hdf5-tools matrix: allow_failures: - os: osx