diff --git a/.cproject b/.cproject
index 23ec7bdc0..1b4d0b1ec 100644
--- a/.cproject
+++ b/.cproject
@@ -582,6 +582,7 @@
make
+
tests/testBayesTree.run
true
false
@@ -589,6 +590,7 @@
make
+
testBinaryBayesNet.run
true
false
@@ -636,6 +638,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -643,6 +646,7 @@
make
+
tests/testSymbolicFactor.run
true
false
@@ -650,6 +654,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -665,6 +670,7 @@
make
+
tests/testBayesTree
true
false
@@ -1000,6 +1006,7 @@
make
+
testErrors.run
true
false
@@ -1229,6 +1236,46 @@
true
true
+
+ make
+ -j5
+ testBTree.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testDSF.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testDSFMap.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testDSFVector.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testFixedVector.run
+ true
+ true
+ true
+
make
-j2
@@ -1311,7 +1358,6 @@
make
-
testSimulated2DOriented.run
true
false
@@ -1351,7 +1397,6 @@
make
-
testSimulated2D.run
true
false
@@ -1359,7 +1404,6 @@
make
-
testSimulated3D.run
true
false
@@ -1373,46 +1417,6 @@
true
true
-
- make
- -j5
- testBTree.run
- true
- true
- true
-
-
- make
- -j5
- testDSF.run
- true
- true
- true
-
-
- make
- -j5
- testDSFMap.run
- true
- true
- true
-
-
- make
- -j5
- testDSFVector.run
- true
- true
- true
-
-
- make
- -j5
- testFixedVector.run
- true
- true
- true
-
make
-j5
@@ -1670,6 +1674,7 @@
cpack
+
-G DEB
true
false
@@ -1677,6 +1682,7 @@
cpack
+
-G RPM
true
false
@@ -1684,6 +1690,7 @@
cpack
+
-G TGZ
true
false
@@ -1691,6 +1698,7 @@
cpack
+
--config CPackSourceConfig.cmake
true
false
@@ -2417,6 +2425,7 @@
make
+
testGraph.run
true
false
@@ -2424,6 +2433,7 @@
make
+
testJunctionTree.run
true
false
@@ -2431,6 +2441,7 @@
make
+
testSymbolicBayesNetB.run
true
false
@@ -2620,6 +2631,14 @@
true
true
+
+ make
+ -j5
+ testInitializePose3.run
+ true
+ true
+ true
+
make
-j2
@@ -2894,7 +2913,6 @@
make
-
tests/testGaussianISAM2
true
false
diff --git a/examples/Data/pose3example-grid.txt b/examples/Data/pose3example-grid.txt
new file mode 100644
index 000000000..854fa0ccb
--- /dev/null
+++ b/examples/Data/pose3example-grid.txt
@@ -0,0 +1,71 @@
+VERTEX_SE3:QUAT 0 1.63791e-12 7.56548e-14 -3.02811e-12 5.35657e-13 2.43616e-13 9.71152e-14 1
+VERTEX_SE3:QUAT 1 1.01609 0.00274307 -0.0351514 -0.499545 0.247735 0.723569 -0.406854
+VERTEX_SE3:QUAT 2 1.99996 0.0304956 -0.040662 0.403501 -0.294714 -0.4254 0.754563
+VERTEX_SE3:QUAT 3 1.94371 1.06535 0.0118614 -0.0471731 -0.541615 0.820893 0.17482
+VERTEX_SE3:QUAT 4 0.962753 0.999477 0.0211017 -0.19663 -0.66009 0.470743 0.551379
+VERTEX_SE3:QUAT 5 -0.00956768 0.965396 -0.021854 -0.320221 -0.518368 0.47521 0.634766
+VERTEX_SE3:QUAT 6 -0.0863793 1.97682 0.000531117 -0.0173439 -0.573793 -0.450627 0.683663
+VERTEX_SE3:QUAT 7 0.918905 2.01556 -0.0139773 0.56169 -0.440513 0.199057 0.671438
+VERTEX_SE3:QUAT 8 1.92094 2.05524 0.0469884 0.0073084 -0.372357 -0.467582 0.801663
+VERTEX_SE3:QUAT 9 1.86182 2.05449 1.09237 0.0131731 -0.05784 0.0335652 0.997674
+VERTEX_SE3:QUAT 10 0.880176 2.02406 1.00997 -0.39342 -0.287909 0.757918 0.433462
+VERTEX_SE3:QUAT 11 -0.0960463 1.98653 0.995791 0.434103 -0.199044 0.585176 0.655367
+VERTEX_SE3:QUAT 12 -0.0911401 0.997117 0.988217 -0.0925477 0.572872 0.537294 0.612019
+VERTEX_SE3:QUAT 13 0.948316 1.02239 0.991745 0.142484 0.560062 0.750078 0.321578
+VERTEX_SE3:QUAT 14 1.92631 1.08945 1.06749 0.23878 0.380837 0.796564 -0.404269
+VERTEX_SE3:QUAT 15 1.95398 0.0777667 0.982353 -0.384392 0.58733 0.685207 -0.194366
+VERTEX_SE3:QUAT 16 0.946032 0.0482667 0.952308 -0.218979 0.186315 -0.494185 0.820437
+VERTEX_SE3:QUAT 17 -0.0625076 -0.034424 0.942171 0.514725 -0.185043 -0.44771 0.707371
+VERTEX_SE3:QUAT 18 -0.083807 -0.0106666 1.9853 0.00792651 1.98919e-05 -0.00128106 0.999968
+VERTEX_SE3:QUAT 19 0.918067 -0.000897795 1.92157 -0.342141 0.241241 -0.726975 0.544288
+VERTEX_SE3:QUAT 20 1.90041 0.0323631 2.00636 0.412572 -0.0930131 -0.133075 0.896339
+VERTEX_SE3:QUAT 21 1.84895 1.05013 2.0738 -0.580757 0.35427 0.729393 -0.0721062
+VERTEX_SE3:QUAT 22 0.880221 1.00671 1.99021 0.147752 0.355662 0.917953 0.095058
+VERTEX_SE3:QUAT 23 -0.0950872 1.00374 1.95013 -0.29909 -0.0578461 0.857019 0.415594
+VERTEX_SE3:QUAT 24 -0.111581 1.97979 1.98762 0.565153 0.214463 -0.523058 0.600848
+VERTEX_SE3:QUAT 25 0.837568 2.01589 2.03075 -0.284756 0.369992 0.875484 -0.124692
+VERTEX_SE3:QUAT 26 1.82708 2.05081 2.07052 0.254696 0.250865 0.653216 0.667462
+EDGE_SE3:QUAT 0 1 1.00497 0.002077 -0.015539 -0.508004 0.250433 0.711222 -0.416386 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 1 2 -0.200593 0.339956 -0.908079 -0.093598 0.151993 0.42829 0.885836 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 2 3 -0.922791 0.330629 -0.292682 0.365657 -0.051986 0.924849 -0.090813 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 3 4 0.893075 0.246476 0.331154 -0.285927 0.341221 -0.267609 0.854517 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 4 5 0.280674 0.244242 0.923726 0.035064 0.21101 0.083834 0.973251 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 5 6 0.955621 0.355669 -0.025152 -0.306713 0.131221 -0.781587 0.527096 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 6 7 -0.076631 0.636081 -0.771439 0.702021 0.326514 0.122181 0.620988 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 7 8 0.582761 -0.721177 -0.376875 -0.733841 -0.170725 -0.256653 0.605359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 8 9 0.600312 0.298765 0.767014 0.057612 0.332574 0.486324 0.805956 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 9 10 -0.986649 0.03008 -0.008766 -0.362177 -0.253215 0.763748 0.470531 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 10 11 0.275109 0.534769 0.823463 0.450708 -0.472399 -0.432689 0.621677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 11 12 -0.61882 0.024878 0.773748 0.0927029 0.786162 -0.21122 0.573359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 12 13 -0.175537 -0.730832 0.634529 -0.018628 0.006375 0.428306 0.903419 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 13 14 -0.700208 -0.245198 0.637353 -0.035865 0.273394 0.645363 0.712374 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 14 15 0.373495 0.373768 -0.846199 0.400323 0.310362 -0.422222 0.751762 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 15 16 0.648588 0.157829 0.72252 0.781502 -0.210141 -0.501005 -0.30674 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 16 17 -0.390339 -0.702656 -0.572321 0.765815 0.055816 0.032478 0.63981 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 17 18 -0.261114 0.908685 0.421318 -0.501833 0.166567 0.448468 0.720622 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 18 19 1.00815 0.012634 -0.029822 -0.347007 0.205082 -0.740641 0.537569 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 19 20 -0.162376 0.581623 0.810804 0.628338 0.075411 0.650639 0.41973 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 20 21 -0.358942 0.627689 -0.704045 -0.469133 0.542456 0.530583 -0.451816 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 21 22 0.362417 0.298352 0.854822 0.004058 -0.696926 0.140345 0.703265 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 22 23 0.934942 0.020321 -0.358044 -0.445461 0.260916 -0.379862 0.767589 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 23 24 0.741887 -0.657659 0.215293 -0.584859 0.196138 0.688031 0.38221 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 24 25 0.300145 0.82011 -0.39974 0.46538 -0.593595 -0.202131 0.624668 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 25 26 -0.85591 0.022701 -0.510794 0.12929 -0.685192 -0.503707 0.509978 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 0 5 0.026721 0.990497 -0.007651 -0.317476 -0.510239 0.467341 0.648427 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 3 8 0.390516 -0.401461 -0.830724 0.503106 -0.367814 0.780584 0.047806 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 4 1 -0.813838 -0.446181 0.319175 0.224903 -0.031827 0.97265 0.048561 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 4 13 0.571273 -0.805401 0.077339 0.892031 0.329761 0.275468 0.140201 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 5 12 0.389794 -0.882655 0.268063 0.712423 0.550662 0.275339 0.33677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 6 11 0.800298 0.505022 0.361738 0.739335 0.419366 0.443817 0.283801 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 10 13 -0.912531 0.430955 -0.018942 0.830493 -0.093519 0.272041 0.477001 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 12 23 -0.797606 0.437737 0.311476 -0.657137 -0.196625 0.136652 0.714728 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 13 22 -0.116836 0.952032 0.269398 -0.216437 0.086571 0.260965 0.936781 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 14 21 0.749295 0.373389 0.581641 0.253048 0.511007 -0.537262 0.621439 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 16 1 0.160985 0.555966 -0.811911 0.748057 0.122381 -0.369631 0.537407 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 18 23 0.028909 1.02689 -0.00265 -0.294167 -0.071607 0.850901 0.429308 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 19 16 -0.230711 0.750637 -0.607511 0.14647 -0.102538 0.297899 0.937704 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 20 15 -0.031986 -0.741129 -0.728721 -0.278926 0.731172 0.404675 -0.473103 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 22 19 -0.332601 0.704401 -0.687251 -0.372165 -0.054346 0.713024 0.591725 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 22 25 0.347067 -0.634646 0.657147 0.018567 0.476762 0.040939 0.877882 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 25 10 0.388971 -0.723981 -0.559653 -0.373459 -0.014654 -0.696123 0.612965 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 26 21 -0.979482 -0.024822 0.043763 -0.326753 0.819942 0.292615 0.367837 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
diff --git a/examples/Data/pose3exampleGrid.g2o b/examples/Data/pose3exampleGrid.g2o
deleted file mode 100644
index c93ee9c19..000000000
--- a/examples/Data/pose3exampleGrid.g2o
+++ /dev/null
@@ -1,77 +0,0 @@
-VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
-VERTEX_SE3:QUAT 1 0.968478 -0.015273 -0.024009 -0.568468 -0.224707 0.094925 0.785710
-VERTEX_SE3:QUAT 2 1.991529 -0.016938 -0.045515 -0.542127 -0.179147 -0.494163 0.655597
-VERTEX_SE3:QUAT 3 2.026568 0.958559 -0.048399 0.363597 0.453322 -0.812746 0.041715
-VERTEX_SE3:QUAT 4 0.987528 0.968615 -0.085312 -0.082238 0.306441 0.936656 0.148344
-VERTEX_SE3:QUAT 5 -0.031537 0.995686 -0.066640 0.335085 0.235204 -0.686500 0.600929
-VERTEX_SE3:QUAT 6 -0.002507 1.983505 -0.158449 -0.401363 0.515966 0.711353 0.258194
-VERTEX_SE3:QUAT 7 1.018807 1.972634 -0.131017 0.323170 -0.282056 0.470606 0.771062
-VERTEX_SE3:QUAT 8 1.994170 1.909926 -0.104255 -0.137653 -0.266858 -0.928006 0.220551
-VERTEX_SE3:QUAT 9 1.910394 1.961603 0.916595 -0.028577 -0.021584 -0.026441 0.999009
-VERTEX_SE3:QUAT 10 0.933629 2.024306 0.857267 -0.303946 -0.378940 -0.856390 0.174981
-VERTEX_SE3:QUAT 11 -0.061508 2.088874 0.841008 0.562526 -0.203739 0.259658 0.758045
-VERTEX_SE3:QUAT 12 -0.106592 1.071471 0.945747 -0.397133 0.277162 -0.755756 0.440794
-VERTEX_SE3:QUAT 13 0.897260 1.009421 1.068154 -0.534411 0.418094 -0.189137 0.709809
-VERTEX_SE3:QUAT 14 1.907694 0.966386 1.128843 0.576449 -0.149518 0.310096 0.741074
-VERTEX_SE3:QUAT 15 1.827009 -0.065936 1.212580 0.440256 -0.304111 0.217991 0.816193
-VERTEX_SE3:QUAT 16 0.811179 -0.009700 1.175299 0.228574 0.021414 0.687899 0.688543
-VERTEX_SE3:QUAT 17 -0.207493 0.046063 1.198636 0.083786 0.149500 0.278888 0.944908
-VERTEX_SE3:QUAT 18 -0.239598 0.069543 2.175883 -0.020534 -0.002367 -0.032999 0.999242
-VERTEX_SE3:QUAT 19 0.795282 0.024403 2.172337 -0.059128 -0.663282 -0.736161 0.120951
-VERTEX_SE3:QUAT 20 1.795463 -0.039545 2.189997 0.262525 0.561623 0.108661 0.777080
-VERTEX_SE3:QUAT 21 1.838289 0.918009 2.142889 0.391707 0.271811 0.534284 0.698015
-VERTEX_SE3:QUAT 22 0.843244 0.956966 2.182675 0.175401 -0.294535 0.796879 0.497462
-VERTEX_SE3:QUAT 23 -0.184445 0.969193 2.212233 -0.022667 -0.309094 0.188130 0.931963
-VERTEX_SE3:QUAT 24 -0.188460 1.988074 2.109480 0.280735 -0.333968 -0.767404 0.469834
-VERTEX_SE3:QUAT 25 0.832834 1.987249 2.119934 0.409102 -0.211526 0.520233 0.719201
-VERTEX_SE3:QUAT 26 1.842345 1.933435 2.082102 0.498150 -0.149112 0.556190 0.648278
-EDGE_SE3:QUAT 0 1 0.968478 -0.015273 -0.024009 -0.568468 -0.224707 0.094925 0.785710 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 1 2 0.895353 0.128330 -0.478510 -0.181316 0.338937 -0.430522 0.816638 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 2 3 -0.429131 -0.043280 0.875673 -0.108628 0.924958 -0.331599 0.150650 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 3 4 0.786542 -0.392786 0.555103 0.731034 -0.219263 -0.010938 0.646060 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 4 5 0.962014 0.324025 0.095456 -0.529804 0.406661 0.542675 0.509350 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 5 6 -0.592671 -0.143889 -0.783004 0.849231 -0.212159 -0.337427 0.346321 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 6 7 -0.578557 -0.780086 -0.317320 -0.050541 -0.889438 -0.373451 0.258610 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 7 8 0.373848 -0.901266 -0.062758 0.564749 0.378680 0.694277 0.235883 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 8 9 0.436073 0.363895 0.853958 0.144187 0.238953 0.925910 0.254564 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 9 10 -0.980372 0.013025 -0.014878 -0.307108 -0.358351 -0.855183 0.214316 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 10 11 0.735233 -0.578374 -0.345900 -0.601711 -0.654423 -0.419528 0.183497 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 11 12 -0.139622 -0.130002 1.005841 0.631012 0.022105 0.762353 0.141962 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 12 13 -0.198544 0.382112 0.917128 -0.217232 -0.341211 0.470993 0.783933 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 13 14 0.591922 -0.251242 0.782954 0.703837 -0.472659 0.521380 0.096797 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 14 15 -0.309581 -0.032947 0.991103 -0.205941 -0.114195 0.017928 0.971713 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 15 16 -0.752100 0.640733 0.245499 0.097291 0.479897 0.332422 0.806058 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 16 17 0.006363 0.960206 -0.345414 -0.061422 0.088813 -0.490352 0.864809 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 17 18 -0.243141 0.271842 0.907511 -0.098852 -0.148661 -0.312729 0.932914 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 18 19 1.035565 0.023491 -0.008895 -0.036454 -0.649327 -0.745091 0.147935 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 19 20 -0.953182 0.279364 -0.134773 0.263672 -0.770187 -0.444277 0.374040 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 20 21 0.497302 0.783380 -0.244889 -0.149390 -0.083102 0.487968 0.855958 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 21 22 -0.241016 0.568368 -0.782358 -0.446393 -0.122376 0.453495 0.761643 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 22 23 0.481169 0.908199 0.028604 -0.365642 0.171794 -0.588182 0.700596 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 23 24 0.310272 0.963355 -0.156115 -0.027745 -0.201443 -0.897925 0.390360 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 24 25 -0.409898 0.553347 -0.754283 0.326372 0.600803 0.719096 0.124169 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 25 26 0.314171 -0.950746 0.144281 0.133133 -0.001730 0.018387 0.990926 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 0 5 0.003921 0.989262 -0.008668 0.344972 0.271052 -0.662312 0.607345 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 0 17 -0.004748 -0.016661 1.014253 0.095179 0.169387 0.315461 0.928834 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 1 4 0.413069 0.396669 0.824709 0.266584 -0.252890 0.915410 0.164328 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 3 8 0.265427 -0.541821 -0.818705 0.520335 -0.589688 0.189278 0.587956 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 3 14 -0.593398 -0.729533 0.348548 0.279515 -0.277200 -0.916882 0.066028 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 6 11 -0.866710 0.505129 0.215695 0.172623 -0.951867 -0.249430 0.044004 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 7 10 0.743464 0.292393 0.573155 0.712782 0.375290 0.526276 0.272273 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 9 14 -0.011444 -1.026550 0.016772 0.610325 -0.121904 0.314891 0.716580 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 9 26 -0.033212 -0.029706 0.975351 0.510053 -0.198209 0.557527 0.624278 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 10 25 0.655684 0.485916 0.598187 -0.700078 -0.428078 -0.464953 0.332353 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 12 17 0.930615 0.369372 0.051025 0.230774 -0.278742 0.911677 0.194659 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 12 23 0.306720 -0.815553 0.415865 0.581204 -0.505705 0.604084 0.203831 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 13 4 0.474778 0.869505 -0.090395 -0.410825 -0.369465 0.822956 0.132141 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 13 16 0.632289 -0.415884 -0.668153 0.221431 -0.613167 0.709932 0.266441 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 13 22 -0.442996 -0.863878 0.046061 -0.079225 0.840832 -0.535376 0.009883 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 15 20 0.632455 0.661923 0.364197 0.030730 0.661379 -0.405142 0.630470 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 16 1 -0.307083 -0.356121 -0.862632 -0.732371 0.242194 -0.446749 0.453200 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 16 19 0.347792 0.365913 0.878300 0.501749 0.573409 0.434965 0.479850 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 18 23 0.027959 1.027767 -0.055691 0.034139 -0.330077 0.166677 0.928495 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 19 22 -0.173539 -0.125423 0.957399 -0.799298 -0.382195 -0.336755 0.318821 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 21 14 -0.104907 -0.825889 -0.563170 -0.004285 -0.468249 0.018532 0.883392 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 21 26 0.956031 0.050291 -0.309699 -0.152297 -0.325772 0.225738 0.905384 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 24 11 0.143525 -0.745793 -0.608698 0.263093 0.680516 0.559568 0.393147 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
-EDGE_SE3:QUAT 25 22 -0.512781 -0.078741 0.857967 -0.053138 0.120352 0.410455 0.902341 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000
diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index bed24fb1e..578b78dd5 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -170,6 +170,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
}
// add prior
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
+ initialPose.insert(keyAnchor, Pose3());
pose3graph.add(PriorFactor(keyAnchor, Pose3(), priorModel));
// Create optimizer
@@ -180,10 +181,10 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// put into Values structure
Values estimate;
- BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
+ BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) {
Key key = key_value.key;
if (key != keyAnchor) {
- const Pose3& pose = estimate.at(key);
+ const Pose3& pose = GNresult.at(key);
estimate.insert(key, pose);
}
}
diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp
index 77cfe8e12..355fb1238 100644
--- a/gtsam/slam/tests/testInitializePose3.cpp
+++ b/gtsam/slam/tests/testInitializePose3.cpp
@@ -103,6 +103,21 @@ TEST( InitializePose3, posesWithGivenGuess ) {
EXPECT(assert_equal(givenPoses, initial, 1e-6));
}
+/* ************************************************************************* */
+TEST( InitializePose3, initializePoses )
+{
+ const string g2oFile = findExampleDataFile("pose3example-grid");
+ NonlinearFactorGraph::shared_ptr inputGraph;
+ Values::shared_ptr expectedValues;
+ bool is3D = true;
+ boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
+ noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
+ inputGraph->add(PriorFactor(0, Pose3(), priorModel));
+
+ Values initial = InitializePose3::initialize(*inputGraph);
+ EXPECT(assert_equal(*expectedValues,initial,1e-4));
+}
+
/* ************************************************************************* */
int main() {