12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

finished and tested initializer

release/4.3a0
Luca 2014-08-19 21:09:51 -04:00
parent fdb31dbd8d
commit 889bbea8d4
5 changed files with 151 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.cproject
View File

@ -582,6 +582,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -589,6 +590,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -636,6 +638,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -643,6 +646,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -650,6 +654,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -665,6 +670,7 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1000,6 +1006,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1229,6 +1236,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1311,7 +1358,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1351,7 +1397,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1359,7 +1404,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1373,46 +1417,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1670,6 +1674,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1677,6 +1682,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1684,6 +1690,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1691,6 +1698,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2417,6 +2425,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2424,6 +2433,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2431,6 +2441,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2620,6 +2631,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInitializePose3.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInitializePose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2894,7 +2913,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

71
examples/Data/pose3example-grid.txt Normal file
View File

@ -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

77
examples/Data/pose3exampleGrid.g2o
View File

@ -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

5
gtsam/slam/InitializePose3.cpp
View File

@ -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<Pose3>(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<Pose3>(key);
const Pose3& pose = GNresult.at<Pose3>(key);
estimate.insert(key, pose);
}
}

15
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -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<Pose3>(0, Pose3(), priorModel));
Values initial = InitializePose3::initialize(*inputGraph);
EXPECT(assert_equal(*expectedValues,initial,1e-4));
}
/* ************************************************************************* */
int main() {