|
| 1 | +# using GraphPlot |
| 2 | +# using Neo4j |
| 3 | +# using DistributedFactorGraphs |
| 4 | +# using IncrementalInference |
| 5 | +# using Test |
| 6 | + |
| 7 | + |
| 8 | +if testDFGAPI == CloudGraphsDFG |
| 9 | + DistributedFactorGraphs.CloudGraphsDFG{SolverParams}() = CloudGraphsDFG{SolverParams}("localhost", 7474, "neo4j", "test", |
| 10 | + "testUser", "testRobot", "testSession", |
| 11 | + nothing, |
| 12 | + nothing, |
| 13 | + IncrementalInference.decodePackedType, |
| 14 | + IncrementalInference.rebuildFactorMetadata!, |
| 15 | + solverParams=SolverParams()) |
| 16 | + |
| 17 | + |
| 18 | + dfg = testDFGAPI{SolverParams}() |
| 19 | + clearRobot!!(dfg) |
| 20 | +else |
| 21 | + dfg = testDFGAPI{NoSolverParams}() |
| 22 | +end |
| 23 | + |
| 24 | + |
| 25 | +v1 = addVariable!(dfg, :a, ContinuousScalar, labels = [:POSE]) |
| 26 | +v2 = addVariable!(dfg, :b, ContinuousScalar, labels = [:LANDMARK]) |
| 27 | +f1 = addFactor!(dfg, [:a; :b], LinearConditional(Normal(50.0,2.0)) ) |
| 28 | +# f1 = addFactor!(fg,[:x0], Prior( pd ) ) |
| 29 | + |
| 30 | +# @testset "Creating Graphs" begin |
| 31 | +global dfg,v1,v2,f1 |
| 32 | + |
| 33 | +@test_throws Exception addFactor!(dfg, DFGFactor{Int, :Symbol}("f2"), [v1, DFGVariable("Nope")]) |
| 34 | +# end |
| 35 | + |
| 36 | +#test before anything changes |
| 37 | +@testset "Producing Dot Files" begin |
| 38 | + |
| 39 | + @test toDot(dfg) == "graph graphname {\n2 [\"label\"=\"b\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n2 -- 3\n3 [\"label\"=\"abf1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n1 [\"label\"=\"a\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n1 -- 3\n}\n" |
| 40 | + @test toDotFile(dfg, "something.dot") == nothing |
| 41 | + Base.rm("something.dot") |
| 42 | + |
| 43 | +end |
| 44 | + |
| 45 | +@testset "Adding Removing Nodes" begin |
| 46 | + dfg2 = testDFGAPI{NoSolverParams}() |
| 47 | + v1 = DFGVariable(:a) |
| 48 | + v2 = DFGVariable(:b) |
| 49 | + v3 = DFGVariable(:c) |
| 50 | + f1 = DFGFactor{ContinuousScalar, :Symbol}(:abf1) |
| 51 | + f2 = DFGFactor{ContinuousScalar, :Symbol}(:f2) |
| 52 | + # @testset "Creating Graphs" begin |
| 53 | + @test addVariable!(dfg2, v1) |
| 54 | + @test addVariable!(dfg2, v2) |
| 55 | + @test_throws ErrorException updateVariable!(dfg2, v3) |
| 56 | + @test addVariable!(dfg2, v3) |
| 57 | + @test_throws ErrorException addVariable!(dfg2, v3) |
| 58 | + @test addFactor!(dfg2, [v1, v2], f1) |
| 59 | + @test_throws ErrorException addFactor!(dfg2, [v1, v2], f1) |
| 60 | + @test_throws ErrorException updateFactor!(dfg2, f2) |
| 61 | + @test addFactor!(dfg2, [:b, :c], f2) |
| 62 | + @test deleteVariable!(dfg2, v3) == v3 |
| 63 | + @test symdiff(ls(dfg2),[:a,:b]) == [] |
| 64 | + @test deleteFactor!(dfg2, f2) == f2 |
| 65 | + @test lsf(dfg2) == [:abf1] |
| 66 | +end |
| 67 | + |
| 68 | +@testset "Listing Nodes" begin |
| 69 | + global dfg,v1,v2,f1 |
| 70 | + @test length(ls(dfg)) == 2 |
| 71 | + @test length(lsf(dfg)) == 1 |
| 72 | + @test symdiff([:a, :b], getVariableIds(dfg)) == [] |
| 73 | + @test getFactorIds(dfg) == [:abf1] |
| 74 | + # |
| 75 | + @test lsf(dfg, :a) == [f1.label] |
| 76 | + # Tags |
| 77 | + @test ls(dfg, tags=[:POSE]) == [:a] |
| 78 | + @test symdiff(ls(dfg, tags=[:POSE, :LANDMARK]), ls(dfg, tags=[:VARIABLE])) == [] |
| 79 | + # Regexes |
| 80 | + @test ls(dfg, r"a") == [v1.label] |
| 81 | + @test lsf(dfg, r"f*") == [f1.label] |
| 82 | + # Accessors |
| 83 | + @test getAddHistory(dfg) == [:a, :b] #, :abf1 |
| 84 | + @test getDescription(dfg) != nothing |
| 85 | + @test getLabelDict(dfg) != nothing |
| 86 | + # Existence |
| 87 | + @test exists(dfg, :a) == true |
| 88 | + @test exists(dfg, v1) == true |
| 89 | + @test exists(dfg, :nope) == false |
| 90 | + # Sorting of results |
| 91 | + # TODO - this function needs to be cleaned up |
| 92 | + unsorted = [:x1_3;:x1_6;:l1;:april1] #this will not work for :x1x2f1 |
| 93 | + @test sortDFG(unsorted) == sortVarNested(unsorted) |
| 94 | + @test_skip sortDFG([:x1x2f1, :x1l1f1]) == [:x1l1f1, :x1x2f1] |
| 95 | +end |
| 96 | + |
| 97 | +# Gets |
| 98 | +@testset "Gets, Sets, and Accessors" begin |
| 99 | + global dfg,v1,v2,f1 |
| 100 | + @test getVariable(dfg, v1.label) == v1 |
| 101 | + @test getFactor(dfg, f1.label) == f1 |
| 102 | + @test_throws Exception getVariable(dfg, :nope) |
| 103 | + @test_throws Exception getVariable(dfg, "nope") |
| 104 | + @test_throws Exception getFactor(dfg, :nope) |
| 105 | + @test_throws Exception getFactor(dfg, "nope") |
| 106 | + |
| 107 | + # Sets |
| 108 | + v1Prime = deepcopy(v1) |
| 109 | + @test updateVariable!(dfg, v1Prime) != v1 |
| 110 | + f1Prime = deepcopy(f1) |
| 111 | + @test updateFactor!(dfg, f1Prime) != f1 |
| 112 | + |
| 113 | + # Accessors |
| 114 | + @test label(v1) == v1.label |
| 115 | + @test tags(v1) == v1.tags |
| 116 | + @test timestamp(v1) == v1.timestamp |
| 117 | + @test estimates(v1) == v1.estimateDict |
| 118 | + @test estimate(v1, :notfound) == nothing |
| 119 | + @test solverData(v1) === v1.solverDataDict[:default] |
| 120 | + @test getData(v1) === v1.solverDataDict[:default] |
| 121 | + @test solverData(v1, :default) === v1.solverDataDict[:default] |
| 122 | + @test solverDataDict(v1) == v1.solverDataDict |
| 123 | + @test internalId(v1) == v1._internalId |
| 124 | + |
| 125 | + @test label(f1) == f1.label |
| 126 | + @test tags(f1) == f1.tags |
| 127 | + @test solverData(f1) == f1.data |
| 128 | + # Deprecated functions |
| 129 | + @test data(f1) == f1.data |
| 130 | + @test getData(f1) == f1.data |
| 131 | + # Internal function |
| 132 | + @test internalId(f1) == f1._internalId |
| 133 | + |
| 134 | + @test getSolverParams(dfg) != nothing |
| 135 | + @test setSolverParams(dfg, getSolverParams(dfg)) == getSolverParams(dfg) |
| 136 | + |
| 137 | + #solver data is initialized |
| 138 | + @test !isInitialized(dfg, :a) |
| 139 | + @test !isInitialized(v2) |
| 140 | + |
| 141 | + @test !isInitialized(v2, key=:second) |
| 142 | + |
| 143 | +end |
| 144 | + |
| 145 | +@testset "Updating Nodes" begin |
| 146 | + global dfg |
| 147 | + #get the variable |
| 148 | + var = getVariable(dfg, :a) |
| 149 | + #make a copy and simulate external changes |
| 150 | + newvar = deepcopy(var) |
| 151 | + estimates(newvar)[:default] = Dict{Symbol, VariableEstimate}( |
| 152 | + :max => VariableEstimate(:default, :max, [100.0]), |
| 153 | + :mean => VariableEstimate(:default, :mean, [50.0]), |
| 154 | + :ppe => VariableEstimate(:default, :ppe, [75.0])) |
| 155 | + #update |
| 156 | + updateVariableSolverData!(dfg, newvar) |
| 157 | + #TODO maybe implement ==; @test newvar==var |
| 158 | + Base.:(==)(varest1::VariableEstimate, varest2::VariableEstimate) = begin |
| 159 | + varest1.lastUpdatedTimestamp == varest2.lastUpdatedTimestamp || return false |
| 160 | + varest1.type == varest2.type || return false |
| 161 | + varest1.solverKey == varest2.solverKey || return false |
| 162 | + varest1.estimate == varest2.estimate || return false |
| 163 | + return true |
| 164 | + end |
| 165 | + #For now spot check |
| 166 | + @test_skip solverDataDict(newvar) == solverDataDict(var) |
| 167 | + @test estimates(newvar) == estimates(var) |
| 168 | + |
| 169 | + # Delete :default and replace to see if new ones can be added |
| 170 | + delete!(estimates(newvar), :default) |
| 171 | + estimates(newvar)[:second] = Dict{Symbol, VariableEstimate}( |
| 172 | + :max => VariableEstimate(:default, :max, [10.0]), |
| 173 | + :mean => VariableEstimate(:default, :mean, [5.0]), |
| 174 | + :ppe => VariableEstimate(:default, :ppe, [7.0])) |
| 175 | + |
| 176 | + # Persist to the original variable. |
| 177 | + updateVariableSolverData!(dfg, newvar) |
| 178 | + # At this point newvar will have only :second, and var should have both (it is the reference) |
| 179 | + @test symdiff(collect(keys(estimates(var))), [:default, :second]) == Symbol[] |
| 180 | + @test symdiff(collect(keys(estimates(newvar))), [:second]) == Symbol[] |
| 181 | + # Get the source too. |
| 182 | + @test symdiff(collect(keys(estimates(getVariable(dfg, :a)))), [:default, :second]) == Symbol[] |
| 183 | +end |
| 184 | + |
| 185 | +# Connectivity test |
| 186 | +@testset "Connectivity Test" begin |
| 187 | + global dfg,v1,v2,f1 |
| 188 | + @test isFullyConnected(dfg) == true |
| 189 | + @test hasOrphans(dfg) == false |
| 190 | + addVariable!(dfg, DFGVariable(:orphan)) |
| 191 | + @test isFullyConnected(dfg) == false |
| 192 | + @test hasOrphans(dfg) == true |
| 193 | +end |
| 194 | + |
| 195 | +# Adjacency matrices |
| 196 | +@testset "Adjacency Matrices" begin |
| 197 | + global dfg,v1,v2,f1 |
| 198 | + # Normal |
| 199 | + adjMat = getAdjacencyMatrix(dfg) |
| 200 | + @test size(adjMat) == (2,4) |
| 201 | + @test symdiff(adjMat[1, :], [nothing, :a, :b, :orphan]) == Symbol[] |
| 202 | + @test symdiff(adjMat[2, :], [:abf1, :abf1, :abf1, nothing]) == Symbol[] |
| 203 | + #sparse |
| 204 | + adjMat, v_ll, f_ll = getAdjacencyMatrixSparse(dfg) |
| 205 | + @test size(adjMat) == (1,3) |
| 206 | + |
| 207 | + # Checking the elements of adjacency, its not sorted so need indexing function |
| 208 | + indexOf = (arr, el1) -> findfirst(el2->el2==el1, arr) |
| 209 | + @test adjMat[1, indexOf(v_ll, :orphan)] == 0 |
| 210 | + @test adjMat[1, indexOf(v_ll, :a)] == 1 |
| 211 | + @test adjMat[1, indexOf(v_ll, :b)] == 1 |
| 212 | + @test symdiff(v_ll, [:a, :b, :orphan]) == Symbol[] |
| 213 | + @test symdiff(f_ll, [:abf1, :abf1, :abf1]) == Symbol[] |
| 214 | +end |
| 215 | + |
| 216 | +# Deletions |
| 217 | +@testset "Deletions" begin |
| 218 | + deleteFactor!(dfg, :abf1) |
| 219 | + @test getFactorIds(dfg) == [] |
| 220 | + deleteVariable!(dfg, :b) |
| 221 | + @test symdiff([:a, :orphan], getVariableIds(dfg)) == [] |
| 222 | + #delete last also for the LightGraphs implementation coverage |
| 223 | + deleteVariable!(dfg, :orphan) |
| 224 | + @test symdiff([:a], getVariableIds(dfg)) == [] |
| 225 | + deleteVariable!(dfg, :a) |
| 226 | + @test getVariableIds(dfg) == [] |
| 227 | +end |
| 228 | + |
| 229 | + |
| 230 | +# Now make a complex graph for connectivity tests |
| 231 | +numNodes = 10 |
| 232 | +dfg = testDFGAPI{NoSolverParams}() |
| 233 | + |
| 234 | +testDFGAPI == CloudGraphsDFG && clearRobot!!(fg) |
| 235 | + |
| 236 | + |
| 237 | +#change ready and backendset for x7,x8 for improved tests on x7x8f1 |
| 238 | +verts = map(n -> addVariable!(dfg, Symbol("x$n"), ContinuousScalar, labels = [:POSE]), 1:numNodes) |
| 239 | +verts[7].ready = 1 |
| 240 | +# verts[7].backendset = 0 |
| 241 | +verts[8].ready = 0 |
| 242 | +verts[8].backendset = 1 |
| 243 | + |
| 244 | +facts = map(n -> addFactor!(dfg, [verts[n], verts[n+1]], LinearConditional(Normal(50.0,2.0))), 1:(numNodes-1)) |
| 245 | + |
| 246 | + |
| 247 | + |
| 248 | +@testset "Getting Neighbors" begin |
| 249 | + global dfg,verts |
| 250 | + # Trivial test to validate that intersect([], []) returns order of first parameter |
| 251 | + @test intersect([:x3, :x2, :x1], [:x1, :x2]) == [:x2, :x1] |
| 252 | + # Get neighbors tests |
| 253 | + @test getNeighbors(dfg, verts[1]) == [:x1x2f1] |
| 254 | + neighbors = getNeighbors(dfg, getFactor(dfg, :x1x2f1)) |
| 255 | + @test neighbors == [:x1, :x2] |
| 256 | + # Testing aliases |
| 257 | + @test getNeighbors(dfg, getFactor(dfg, :x1x2f1)) == ls(dfg, getFactor(dfg, :x1x2f1)) |
| 258 | + @test getNeighbors(dfg, :x1x2f1) == ls(dfg, :x1x2f1) |
| 259 | + |
| 260 | + # ready and backendset |
| 261 | + @test getNeighbors(dfg, :x5, ready=1) == Symbol[] |
| 262 | + @test getNeighbors(dfg, :x5, ready=0) == [:x4x5f1,:x5x6f1] |
| 263 | + @test getNeighbors(dfg, :x5, backendset=1) == Symbol[] |
| 264 | + @test getNeighbors(dfg, :x5, backendset=0) == [:x4x5f1,:x5x6f1] |
| 265 | + @test getNeighbors(dfg, :x7x8f1, ready=0) == [:x8] |
| 266 | + @test getNeighbors(dfg, :x7x8f1, backendset=0) == [:x7] |
| 267 | + @test getNeighbors(dfg, :x7x8f1, ready=1) == [:x7] |
| 268 | + @test getNeighbors(dfg, :x7x8f1, backendset=1) == [:x8] |
| 269 | + @test getNeighbors(dfg, verts[1], ready=0) == [:x1x2f1] |
| 270 | + @test getNeighbors(dfg, verts[1], ready=1) == Symbol[] |
| 271 | + @test getNeighbors(dfg, verts[1], backendset=0) == [:x1x2f1] |
| 272 | + @test getNeighbors(dfg, verts[1], backendset=1) == Symbol[] |
| 273 | + |
| 274 | +end |
| 275 | + |
| 276 | +@testset "Getting Subgraphs" begin |
| 277 | + # Subgraphs |
| 278 | + dfgSubgraph = getSubgraphAroundNode(dfg, verts[1], 2) |
| 279 | + # Only returns x1 and x2 |
| 280 | + @test symdiff([:x1, :x1x2f1, :x2], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) == [] |
| 281 | + # Test include orphan factorsVoid |
| 282 | + dfgSubgraph = getSubgraphAroundNode(dfg, verts[1], 1, true) |
| 283 | + @test symdiff([:x1, :x1x2f1], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) == [] |
| 284 | + # Test adding to the dfg |
| 285 | + dfgSubgraph = getSubgraphAroundNode(dfg, verts[1], 2, true, dfgSubgraph) |
| 286 | + @test symdiff([:x1, :x1x2f1, :x2], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) == [] |
| 287 | + # |
| 288 | + dfgSubgraph = getSubgraph(dfg,[:x1, :x2, :x1x2f1]) |
| 289 | + # Only returns x1 and x2 |
| 290 | + @test symdiff([:x1, :x1x2f1, :x2], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) == [] |
| 291 | + |
| 292 | + # DFG issue #95 - confirming that getSubgraphAroundNode retains order |
| 293 | + # REF: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/95 |
| 294 | + for fId in getVariableIds(dfg) |
| 295 | + # Get a subgraph of this and it's related factors+variables |
| 296 | + dfgSubgraph = getSubgraphAroundNode(dfg, verts[1], 2) |
| 297 | + # For each factor check that the order the copied graph == original |
| 298 | + for fact in getFactors(dfgSubgraph) |
| 299 | + @test fact._variableOrderSymbols == getFactor(dfg, fact.label)._variableOrderSymbols |
| 300 | + end |
| 301 | + end |
| 302 | +end |
| 303 | + |
| 304 | +@testset "Summaries and Summary Graphs" begin |
| 305 | + factorFields = fieldnames(DFGFactorSummary) |
| 306 | + variableFields = fieldnames(DFGVariableSummary) |
| 307 | + |
| 308 | + summary = getSummary(dfg) |
| 309 | + @test symdiff(collect(keys(summary.variables)), ls(dfg)) == Symbol[] |
| 310 | + @test symdiff(collect(keys(summary.factors)), lsf(dfg)) == Symbol[] |
| 311 | + |
| 312 | + summaryGraph = getSummaryGraph(dfg) |
| 313 | + @test symdiff(ls(summaryGraph), ls(dfg)) == Symbol[] |
| 314 | + @test symdiff(lsf(summaryGraph), lsf(dfg)) == Symbol[] |
| 315 | + # Check all fields are equal for all variables |
| 316 | + for v in ls(summaryGraph) |
| 317 | + for field in variableFields |
| 318 | + @test getfield(getVariable(dfg, v), field) == getfield(getVariable(summaryGraph, v), field) |
| 319 | + end |
| 320 | + end |
| 321 | + for f in lsf(summaryGraph) |
| 322 | + for field in factorFields |
| 323 | + @test getfield(getFactor(dfg, f), field) == getfield(getFactor(summaryGraph, f), field) |
| 324 | + end |
| 325 | + end |
| 326 | +end |
0 commit comments