Skip to content

Commit 3589149

Browse files
committed
Just adding in an end-to-end test
1 parent 2dae9db commit 3589149

File tree

1 file changed

+29
-0
lines changed

1 file changed

+29
-0
lines changed

test/HexagonalLightGraphs.jl

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
using Revise
2+
using Neo4j # So that DFG initializes the database driver.
3+
using RoME
4+
using DistributedFactorGraphs
5+
using Test
6+
7+
# start with an empty factor graph object
8+
# fg = initfg()
9+
fg = LightGraphsDFG{SolverParams}( solverParams=SolverParams())
10+
11+
# Add the first pose :x0
12+
x0 = addVariable!(fg, :x0, Pose2)
13+
14+
# Add at a fixed location PriorPose2 to pin :x0 to a starting location (10,10, pi/4)
15+
prior = addFactor!(fg, [:x0], PriorPose2( MvNormal([10; 10; 1.0/8.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) )
16+
17+
# Drive around in a hexagon in the cloud
18+
for i in 0:5
19+
psym = Symbol("x$i")
20+
nsym = Symbol("x$(i+1)")
21+
addVariable!(fg, nsym, Pose2)
22+
pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
23+
addFactor!(fg, [psym;nsym], pp )
24+
end
25+
26+
# Alrighty! At this point, we should be able to solve locally...
27+
# perform inference, and remember first runs are slower owing to Julia's just-in-time compiling
28+
# Can do with graph too!
29+
tree, smt, hist = solveTree!(fg)

0 commit comments

Comments
 (0)