Merge pull request #9 from JuliaRobotics/bug/7_addFactor

Bug/7 add factor

Author: GearsAD

src/services/GraphsDFG.jl

@@ -80,7 +80,7 @@ end
     $(SIGNATURES)
 Add a DFGFactor to a DFG.
 """
-function addFactor!(dfg::GraphsDFG, factor::DFGFactor, variables::Vector{DFGVariable})::Bool
+function addFactor!(dfg::GraphsDFG, variables::Vector{DFGVariable}, factor::DFGFactor)::Bool
     if haskey(dfg.labelDict, factor.label)
         error("Factor '$(factor.label)' already exists in the factor graph")
     end
@@ -346,7 +346,7 @@ function _copyIntoGraph!(sourceDFG::GraphsDFG, destDFG::GraphsDFG, variableFacto
 
             # Only if we have all of them should we add it (otherwise strange things may happen on evaluation)
             if includeOrphanFactors || length(factVariables) == length(variables)
-                addFactor!(destDFG, deepcopy(factor.dfgNode), factVariables)
+                addFactor!(destDFG, factVariables, deepcopy(factor.dfgNode))
             end
         end
     end

test/interfaceTests.jl

@@ -7,7 +7,7 @@ f1 = DFGFactor(:f1)
     addVariable!(dfg, v1)
     @test_throws Exception addVariable!(dfg, v1)
     addVariable!(dfg, v2)
-    addFactor!(dfg, f1, [v1, v2])
+    addFactor!(dfg, [v1, v2], f1)
     @test_throws Exception addFactor!(dfg, DFGFactor("f2"), [v1, DFGVariable("Nope")])
 end
 
@@ -71,7 +71,7 @@ numNodes = 10
 dfg = testDFGAPI()
 verts = map(n -> DFGVariable(Symbol("x$n")), 1:numNodes)
 map(v -> addVariable!(dfg, v), verts)
-map(n -> addFactor!(dfg, DFGFactor(Symbol("x$(n)x$(n+1)f1")), [verts[n], verts[n+1]]), 1:(numNodes-1))
+map(n -> addFactor!(dfg, [verts[n], verts[n+1]], DFGFactor(Symbol("x$(n)x$(n+1)f1"))), 1:(numNodes-1))
 # map(n -> addFactor!(dfg, [verts[n], verts[n+2]], DFGFactor(Symbol("x$(n)x$(n+2)f2"))), 1:2:(numNodes-2))
 
 @testset "Getting Neighbors" begin