Skip to content

Instantly share code, notes, and snippets.

@adthoms
Created March 10, 2024 04:54
Show Gist options
  • Save adthoms/23cb1515f6674edbbf166a562859d03a to your computer and use it in GitHub Desktop.
Save adthoms/23cb1515f6674edbbf166a562859d03a to your computer and use it in GitHub Desktop.
factor_graph_2d_four_landmarks_high_noise.pyfg
EDGE_SE2 0.000000000 X0 X1 0.450803136 0.000285191 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 0.000000000 X0 L0 0.901387819 0.01
EDGE_RANGE 0.000000000 X0 L1 0.901388114 0.01
EDGE_RANGE 0.500000000 X1 L0 1.295024750 0.01
EDGE_RANGE 0.500000000 X1 L1 1.206164147 0.01
EDGE_SE2 0.500000000 X1 X2 0.453387111 -0.000285191 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 1.000000000 X2 L0 1.713037154 0.01
EDGE_RANGE 1.000000000 X2 L1 1.578402256 0.01
EDGE_SE2 1.000000000 X2 X3 0.429707247 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 1.500000000 X3 L0 2.142493355 0.01
EDGE_RANGE 1.500000000 X3 L1 1.980985906 0.01
EDGE_SE2 1.500000000 X3 X4 0.444444444 0.005994706 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 2.000000000 X4 L0 2.577167064 0.01
EDGE_RANGE 2.000000000 X4 L1 2.398074570 0.01
EDGE_SE2 2.000000000 X4 X5 0.444444444 0.001106396 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 2.500000000 X5 L0 3.013984894 0.01
EDGE_RANGE 2.500000000 X5 L1 2.823604961 0.01
EDGE_SE2 2.500000000 X5 X6 0.444444444 -0.014080380 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 3.000000000 X6 L0 3.453242508 0.01
EDGE_RANGE 3.000000000 X6 L1 3.254039104 0.01
EDGE_RANGE 3.000000000 X6 L2 5.880364123 0.01
EDGE_SE2 3.000000000 X6 X7 0.444444444 0.006979278 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 3.500000000 X7 L0 3.893276860 0.01
EDGE_RANGE 3.500000000 X7 L1 3.687715627 0.01
EDGE_RANGE 3.500000000 X7 L2 5.440829299 0.01
EDGE_RANGE 3.500000000 X7 L3 5.661092298 0.01
EDGE_SE2 3.500000000 X7 X8 0.444444444 0.004206426 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 4.000000000 X8 L0 4.334534477 0.01
EDGE_RANGE 4.000000000 X8 L1 4.124321867 0.01
EDGE_RANGE 4.000000000 X8 L2 5.000996883 0.01
EDGE_RANGE 4.000000000 X8 L3 5.218480216 0.01
EDGE_SE2 4.000000000 X8 X9 0.444444444 -0.024515610 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 4.500000000 X9 L0 4.776243294 0.01
EDGE_RANGE 4.500000000 X9 L1 4.561392240 0.01
EDGE_RANGE 4.500000000 X9 L2 4.562643884 0.01
EDGE_RANGE 4.500000000 X9 L3 4.776243294 0.01
EDGE_SE2 4.500000000 X9 X10 0.444444444 0.022573835 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 5.000000000 X10 L0 5.217483904 0.01
EDGE_RANGE 5.000000000 X10 L1 5.000581078 0.01
EDGE_RANGE 5.000000000 X10 L2 4.124321867 0.01
EDGE_RANGE 5.000000000 X10 L3 4.334623609 0.01
EDGE_SE2 5.000000000 X10 X11 0.438110557 0.000040688 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 5.500000000 X11 L0 5.660963669 0.01
EDGE_RANGE 5.500000000 X11 L1 5.440829299 0.01
EDGE_RANGE 5.500000000 X11 L2 3.688173458 0.01
EDGE_RANGE 5.500000000 X11 L3 3.892733234 0.01
EDGE_SE2 5.500000000 X11 X12 0.450778332 0.001000367 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 6.000000000 X12 L1 5.881349996 0.01
EDGE_RANGE 6.000000000 X12 L2 3.254398681 0.01
EDGE_RANGE 6.000000000 X12 L3 3.452616165 0.01
EDGE_SE2 6.000000000 X12 X13 0.446697297 -0.011315663 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 6.500000000 X13 L2 2.823649027 0.01
EDGE_RANGE 6.500000000 X13 L3 3.012947565 0.01
EDGE_SE2 6.500000000 X13 X14 0.445953892 0.008009956 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 7.000000000 X14 L2 2.397743748 0.01
EDGE_RANGE 7.000000000 X14 L3 2.576753868 0.01
EDGE_SE2 7.000000000 X14 X15 0.440682143 -0.008057217 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 7.500000000 X15 L2 1.980754673 0.01
EDGE_RANGE 7.500000000 X15 L3 2.142612318 0.01
EDGE_SE2 7.500000000 X15 X16 0.451263820 0.019007450 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 8.000000000 X16 L2 1.578012499 0.01
EDGE_RANGE 8.000000000 X16 L3 1.713463390 0.01
EDGE_SE2 8.000000000 X16 X17 0.427334306 -0.010950233 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 8.500000000 X17 L2 1.206942390 0.01
EDGE_RANGE 8.500000000 X17 L3 1.295085459 0.01
EDGE_SE2 8.500000000 X17 X18 0.458995362 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 9.000000000 X18 L2 0.901446275 0.01
EDGE_RANGE 9.000000000 X18 L3 0.901498279 0.01
EDGE_SE2 9.000000000 X18 X19 -0.013549700 0.166666667 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 9.500000000 X19 L2 1.043904551 0.01
EDGE_RANGE 9.500000000 X19 L3 1.003466215 0.01
EDGE_SE2 9.500000000 X19 X20 0.010538625 0.166666667 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 10.000000000 X20 L2 1.192262115 0.01
EDGE_RANGE 10.000000000 X20 L3 1.120317703 0.01
EDGE_SE2 10.000000000 X20 X21 0.007100621 0.166666667 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 10.500000000 X21 L2 1.346608421 0.01
EDGE_RANGE 10.500000000 X21 L3 1.249855998 0.01
EDGE_RANGE 10.500000000 X21 L4 3.092284026 0.01
EDGE_RANGE 10.500000000 X21 L5 3.288236609 0.01
EDGE_SE2 10.500000000 X21 X22 -0.008349702 0.166666667 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 11.000000000 X22 L2 1.502095687 0.01
EDGE_RANGE 11.000000000 X22 L3 1.388011232 0.01
EDGE_SE2 11.000000000 X22 X23 0.000000000 0.166666667 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 11.500000000 X23 L2 1.660681233 0.01
EDGE_RANGE 11.500000000 X23 L3 1.529796646 0.01
EDGE_SE2 11.500000000 X23 X24 -0.003632327 0.160431009 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 12.000000000 X24 L2 1.820027472 0.01
EDGE_RANGE 12.000000000 X24 L3 1.677050983 0.01
EDGE_SE2 12.000000000 X24 X25 0.003632327 0.172902324 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 12.500000000 X25 L2 1.980810721 0.01
EDGE_RANGE 12.500000000 X25 L3 1.827921221 0.01
EDGE_SE2 12.500000000 X25 X26 -0.015713303 0.157951231 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 13.000000000 X26 L2 2.142493355 0.01
EDGE_RANGE 13.000000000 X26 L3 1.981112608 0.01
EDGE_SE2 13.000000000 X26 X27 0.015713303 0.175382103 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 13.500000000 X27 L2 2.304811626 0.01
EDGE_RANGE 13.500000000 X27 L3 2.136000936 0.01
EDGE_SE2 13.500000000 X27 X28 0.003354517 0.166666667 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 14.000000000 X28 L2 2.467493997 0.01
EDGE_RANGE 14.000000000 X28 L3 2.292802749 0.01
EDGE_SE2 14.000000000 X28 X29 -0.003354517 0.166666667 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 14.500000000 X29 L2 2.631281867 0.01
EDGE_RANGE 14.500000000 X29 L3 2.452642775 0.01
EDGE_SE2 14.500000000 X29 X30 0.000000000 0.166666667 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 15.000000000 X30 L2 2.795084972 0.01
EDGE_RANGE 15.000000000 X30 L3 2.608845347 0.01
EDGE_SE2 15.000000000 X30 X31 -0.024274769 0.167285929 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 15.500000000 X31 L2 2.959213484 0.01
EDGE_RANGE 15.500000000 X31 L3 2.770879316 0.01
EDGE_SE2 15.500000000 X31 X32 0.031794927 0.152792352 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 16.000000000 X32 L2 3.122747934 0.01
EDGE_RANGE 16.000000000 X32 L3 2.931194330 0.01
EDGE_SE2 16.000000000 X32 X33 -0.007520158 0.189020481 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 16.500000000 X33 L2 3.288530763 0.01
EDGE_RANGE 16.500000000 X33 L3 3.092111086 0.01
EDGE_SE2 16.500000000 X33 X34 0.000000000 0.157567905 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 17.000000000 X34 L2 3.452566814 0.01
EDGE_RANGE 17.000000000 X34 L3 3.254270698 0.01
EDGE_SE2 17.000000000 X34 X35 0.019046846 0.157532602 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 17.500000000 X35 L2 3.618048891 0.01
EDGE_RANGE 17.500000000 X35 L3 3.416666667 0.01
EDGE_SE2 17.500000000 X35 X36 -0.028904728 0.175800731 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 18.000000000 X36 L2 3.784048822 0.01
EDGE_RANGE 18.000000000 X36 L3 3.579540561 0.01
EDGE_SE2 18.000000000 X36 X37 -0.432987506 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 18.500000000 X37 L2 3.867101668 0.01
EDGE_RANGE 18.500000000 X37 L3 3.698185244 0.01
EDGE_SE2 18.500000000 X37 X38 -0.446043500 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 19.000000000 X38 L2 3.998766770 0.01
EDGE_RANGE 19.000000000 X38 L3 3.864196168 0.01
EDGE_SE2 19.000000000 X38 X39 -0.447451489 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 19.500000000 X39 L2 4.173829626 0.01
EDGE_RANGE 19.500000000 X39 L3 4.073504356 0.01
EDGE_SE2 19.500000000 X39 X40 -0.441437400 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 20.000000000 X40 L2 4.387522357 0.01
EDGE_RANGE 20.000000000 X40 L3 4.317367311 0.01
EDGE_SE2 20.000000000 X40 X41 -0.444444444 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 20.500000000 X41 L2 4.634476434 0.01
EDGE_RANGE 20.500000000 X41 L3 4.591743126 0.01
EDGE_SE2 20.500000000 X41 X42 -0.444444444 -0.002894141 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 21.000000000 X42 L2 4.908330934 0.01
EDGE_RANGE 21.000000000 X42 L3 4.891176522 0.01
EDGE_SE2 21.000000000 X42 X43 -0.444444444 0.002894141 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 21.500000000 X43 L2 5.206903247 0.01
EDGE_RANGE 21.500000000 X43 L3 5.213225371 0.01
EDGE_SE2 21.500000000 X43 X44 -0.459897372 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 22.000000000 X44 L2 5.523860500 0.01
EDGE_RANGE 22.000000000 X44 L3 5.548947992 0.01
EDGE_SE2 22.000000000 X44 X45 -0.439841766 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 22.500000000 X45 L0 5.900731774 0.01
EDGE_RANGE 22.500000000 X45 L1 5.857012175 0.01
EDGE_RANGE 22.500000000 X45 L2 5.856358038 0.01
EDGE_RANGE 22.500000000 X45 L3 5.900650279 0.01
EDGE_SE2 22.500000000 X45 X46 -0.433594195 -0.015310361 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 23.000000000 X46 L0 5.548520683 0.01
EDGE_RANGE 23.000000000 X46 L1 5.523588586 0.01
EDGE_SE2 23.000000000 X46 X47 -0.448279147 0.015310361 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 23.500000000 X47 L0 5.211350978 0.01
EDGE_RANGE 23.500000000 X47 L1 5.206125289 0.01
EDGE_SE2 23.500000000 X47 X48 -0.440609742 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 24.000000000 X48 L0 4.891100009 0.01
EDGE_RANGE 24.000000000 X48 L1 4.908098900 0.01
EDGE_SE2 24.000000000 X48 X49 -0.437199751 0.000000000 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 24.500000000 X49 L0 4.590378982 0.01
EDGE_RANGE 24.500000000 X49 L1 4.633671394 0.01
EDGE_SE2 24.500000000 X49 X50 -0.451689137 0.004008956 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 25.000000000 X50 L0 4.317485179 0.01
EDGE_RANGE 25.000000000 X50 L1 4.387132468 0.01
EDGE_SE2 25.000000000 X50 X51 -0.438762103 -0.004008956 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 25.500000000 X51 L0 4.072843622 0.01
EDGE_RANGE 25.500000000 X51 L1 4.174159929 0.01
EDGE_SE2 25.500000000 X51 X52 -0.457588166 0.005241532 0.000000000 0.01 0.0 0.0 0.01 0.0 0.01
EDGE_RANGE 26.000000000 X52 L0 3.864970102 0.01
EDGE_RANGE 26.000000000 X52 L1 3.998857386 0.01
VERTEX_SE2 0.000000000 X0 0.999435840 1.000000000 0.000000000
VERTEX_SE2 0.500000000 X1 1.450238976 1.000285191 0.000000000
VERTEX_SE2 1.000000000 X2 1.903626086 1.000000000 0.000000000
VERTEX_SE2 1.500000000 X3 2.333333333 1.000000000 0.000000000
VERTEX_SE2 2.000000000 X4 2.777777778 1.005994706 0.000000000
VERTEX_SE2 2.500000000 X5 3.222222222 1.007101102 0.000000000
VERTEX_SE2 3.000000000 X6 3.666666667 0.993020722 0.000000000
VERTEX_SE2 3.500000000 X7 4.111111111 1.000000000 0.000000000
VERTEX_SE2 4.000000000 X8 4.555555556 1.004206426 0.000000000
VERTEX_SE2 4.500000000 X9 5.000000000 0.979690817 0.000000000
VERTEX_SE2 5.000000000 X10 5.444444444 1.002264652 0.000000000
VERTEX_SE2 5.500000000 X11 5.882555001 1.002305340 0.000000000
VERTEX_SE2 6.000000000 X12 6.333333333 1.003305708 0.000000000
VERTEX_SE2 6.500000000 X13 6.780030631 0.991990044 0.000000000
VERTEX_SE2 7.000000000 X14 7.225984523 1.000000000 0.000000000
VERTEX_SE2 7.500000000 X15 7.666666667 0.991942783 0.000000000
VERTEX_SE2 8.000000000 X16 8.117930487 1.010950233 0.000000000
VERTEX_SE2 8.500000000 X17 8.545264793 1.000000000 0.000000000
VERTEX_SE2 9.000000000 X18 9.004260156 1.000000000 0.000000000
VERTEX_SE2 9.500000000 X19 8.990710456 1.166666667 0.000000000
VERTEX_SE2 10.000000000 X20 9.001249081 1.333333333 0.000000000
VERTEX_SE2 10.500000000 X21 9.008349702 1.500000000 0.000000000
VERTEX_SE2 11.000000000 X22 9.000000000 1.666666667 0.000000000
VERTEX_SE2 11.500000000 X23 9.000000000 1.833333333 0.000000000
VERTEX_SE2 12.000000000 X24 8.996367673 1.993764343 0.000000000
VERTEX_SE2 12.500000000 X25 9.000000000 2.166666667 0.000000000
VERTEX_SE2 13.000000000 X26 8.984286697 2.324617897 0.000000000
VERTEX_SE2 13.500000000 X27 9.000000000 2.500000000 0.000000000
VERTEX_SE2 14.000000000 X28 9.003354517 2.666666667 0.000000000
VERTEX_SE2 14.500000000 X29 9.000000000 2.833333333 0.000000000
VERTEX_SE2 15.000000000 X30 9.000000000 3.000000000 0.000000000
VERTEX_SE2 15.500000000 X31 8.975725231 3.167285929 0.000000000
VERTEX_SE2 16.000000000 X32 9.007520158 3.320078281 0.000000000
VERTEX_SE2 16.500000000 X33 9.000000000 3.509098762 0.000000000
VERTEX_SE2 17.000000000 X34 9.000000000 3.666666667 0.000000000
VERTEX_SE2 17.500000000 X35 9.019046846 3.824199269 0.000000000
VERTEX_SE2 18.000000000 X36 8.990142117 4.000000000 0.000000000
VERTEX_SE2 18.500000000 X37 8.557154611 4.000000000 0.000000000
VERTEX_SE2 19.000000000 X38 8.111111111 4.000000000 0.000000000
VERTEX_SE2 19.500000000 X39 7.663659622 4.000000000 0.000000000
VERTEX_SE2 20.000000000 X40 7.222222222 4.000000000 0.000000000
VERTEX_SE2 20.500000000 X41 6.777777778 4.000000000 0.000000000
VERTEX_SE2 21.000000000 X42 6.333333333 3.997105859 0.000000000
VERTEX_SE2 21.500000000 X43 5.888888889 4.000000000 0.000000000
VERTEX_SE2 22.000000000 X44 5.428991517 4.000000000 0.000000000
VERTEX_SE2 22.500000000 X45 4.989149751 4.000000000 0.000000000
VERTEX_SE2 23.000000000 X46 4.555555556 3.984689639 0.000000000
VERTEX_SE2 23.500000000 X47 4.107276409 4.000000000 0.000000000
VERTEX_SE2 24.000000000 X48 3.666666667 4.000000000 0.000000000
VERTEX_SE2 24.500000000 X49 3.229466915 4.000000000 0.000000000
VERTEX_SE2 25.000000000 X50 2.777777778 4.004008956 0.000000000
VERTEX_SE2 25.500000000 X51 2.339015674 4.000000000 0.000000000
VERTEX_SE2 26.000000000 X52 1.881427509 4.005241532 0.000000000
VERTEX_XY L0 0.250000000 0.500000000
VERTEX_XY L1 0.500000000 0.250000000
VERTEX_XY L2 9.500000000 0.250000000
VERTEX_XY L3 9.750000000 0.500000000
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment