added pose initializer
Luca
parent
c0f880d52b
commit
f5cc247b1c
122
.cproject
122
.cproject
|
|
@ -582,7 +582,6 @@
|
|||
</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>
|
||||
|
|
@ -590,7 +589,6 @@
|
|||
</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>
|
||||
|
|
@ -638,7 +636,6 @@
|
|||
</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>
|
||||
|
|
@ -646,7 +643,6 @@
|
|||
</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>
|
||||
|
|
@ -654,7 +650,6 @@
|
|||
</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>
|
||||
|
|
@ -670,7 +665,6 @@
|
|||
</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>
|
||||
|
|
@ -1006,7 +1000,6 @@
|
|||
</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>
|
||||
|
|
@ -1236,46 +1229,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="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1358,6 +1311,7 @@
|
|||
</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>
|
||||
|
|
@ -1397,6 +1351,7 @@
|
|||
</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>
|
||||
|
|
@ -1404,6 +1359,7 @@
|
|||
</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>
|
||||
|
|
@ -1417,6 +1373,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="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1674,7 +1670,6 @@
|
|||
</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>
|
||||
|
|
@ -1682,7 +1677,6 @@
|
|||
</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>
|
||||
|
|
@ -1690,7 +1684,6 @@
|
|||
</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>
|
||||
|
|
@ -1698,7 +1691,6 @@
|
|||
</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>
|
||||
|
|
@ -2425,7 +2417,6 @@
|
|||
</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>
|
||||
|
|
@ -2433,7 +2424,6 @@
|
|||
</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>
|
||||
|
|
@ -2441,7 +2431,6 @@
|
|||
</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>
|
||||
|
|
@ -2631,22 +2620,6 @@
|
|||
<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="testLago.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testLago.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>
|
||||
|
|
@ -2823,6 +2796,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testLago.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -2913,6 +2894,7 @@
|
|||
</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>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,77 @@
|
|||
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
|
||||
|
|
@ -19,6 +19,7 @@
|
|||
#include <gtsam/slam/InitializePose3.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
|
@ -156,92 +157,56 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) {
|
|||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//Values computePoses(const NonlinearFactorGraph& pose2graph,
|
||||
// VectorValues& orientationsLago) {
|
||||
// gttic(lago_computePoses);
|
||||
//
|
||||
// // Linearized graph on full poses
|
||||
// GaussianFactorGraph linearPose2graph;
|
||||
//
|
||||
// // We include the linear version of each between factor
|
||||
// BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph) {
|
||||
//
|
||||
// boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
// boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
|
||||
//
|
||||
// if (pose2Between) {
|
||||
// Key key1 = pose2Between->keys()[0];
|
||||
// double theta1 = orientationsLago.at(key1)(0);
|
||||
// double s1 = sin(theta1);
|
||||
// double c1 = cos(theta1);
|
||||
//
|
||||
// Key key2 = pose2Between->keys()[1];
|
||||
// double theta2 = orientationsLago.at(key2)(0);
|
||||
//
|
||||
// double linearDeltaRot = theta2 - theta1
|
||||
// - pose2Between->measured().theta();
|
||||
// linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize
|
||||
//
|
||||
// double dx = pose2Between->measured().x();
|
||||
// double dy = pose2Between->measured().y();
|
||||
//
|
||||
// Vector globalDeltaCart = //
|
||||
// (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy);
|
||||
// Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
|
||||
// Matrix J1 = -I3;
|
||||
// J1(0, 2) = s1 * dx + c1 * dy;
|
||||
// J1(1, 2) = -c1 * dx + s1 * dy;
|
||||
// // Retrieve the noise model for the relative rotation
|
||||
// boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
|
||||
// boost::dynamic_pointer_cast<noiseModel::Diagonal>(
|
||||
// pose2Between->get_noiseModel());
|
||||
//
|
||||
// linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel);
|
||||
// } else {
|
||||
// throw invalid_argument(
|
||||
// "computeLagoPoses: cannot manage non between factor here!");
|
||||
// }
|
||||
// }
|
||||
// // add prior
|
||||
// linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0),
|
||||
// priorPose2Noise);
|
||||
//
|
||||
// // optimize
|
||||
// VectorValues posesLago = linearPose2graph.optimize();
|
||||
//
|
||||
// // put into Values structure
|
||||
// Values initialGuessLago;
|
||||
// BOOST_FOREACH(const VectorValues::value_type& it, posesLago) {
|
||||
// Key key = it.first;
|
||||
// if (key != keyAnchor) {
|
||||
// const Vector& poseVector = it.second;
|
||||
// Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
|
||||
// orientationsLago.at(key)(0) + poseVector(2));
|
||||
// initialGuessLago.insert(key, poseLago);
|
||||
// }
|
||||
// }
|
||||
// return initialGuessLago;
|
||||
//}
|
||||
Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
|
||||
gttic(InitializePose3_computePoses);
|
||||
|
||||
// put into Values structure
|
||||
Values initialPose;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){
|
||||
Key key = key_value.key;
|
||||
const Rot3& rot = initialRot.at<Rot3>(key);
|
||||
Pose3 initializedPose = Pose3(rot, Point3());
|
||||
initialPose.insert(key, initializedPose);
|
||||
}
|
||||
// add prior
|
||||
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
|
||||
pose3graph.add(PriorFactor<Pose3>(keyAnchor, Pose3(), priorModel));
|
||||
|
||||
// Create optimizer
|
||||
GaussNewtonParams params;
|
||||
params.maxIterations = 1;
|
||||
GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
|
||||
Values GNresult = optimizer.optimize();
|
||||
|
||||
// put into Values structure
|
||||
Values estimate;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
|
||||
Key key = key_value.key;
|
||||
if (key != keyAnchor) {
|
||||
const Pose3& pose = estimate.at<Pose3>(key);
|
||||
estimate.insert(key, pose);
|
||||
}
|
||||
}
|
||||
return estimate;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
|
||||
// gttic(lago_initialize);
|
||||
//
|
||||
// // We "extract" the Pose2 subgraph of the original graph: this
|
||||
// // is done to properly model priors and avoiding operating on a larger graph
|
||||
// NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
|
||||
//
|
||||
// // Get orientations from relative orientation measurements
|
||||
// VectorValues orientationsLago = computeOrientations(pose2Graph,
|
||||
// useOdometricPath);
|
||||
//
|
||||
// // Compute the full poses
|
||||
// return computePoses(pose2Graph, orientationsLago);
|
||||
//}
|
||||
//
|
||||
/* ************************************************************************* */
|
||||
Values initialize(const NonlinearFactorGraph& graph,
|
||||
const Values& givenGuess) {
|
||||
Values initialize(const NonlinearFactorGraph& graph) {
|
||||
gttic(InitializePose3_initialize);
|
||||
|
||||
// We "extract" the Pose3 subgraph of the original graph: this
|
||||
// is done to properly model priors and avoiding operating on a larger graph
|
||||
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
|
||||
|
||||
// Get orientations from relative orientation measurements
|
||||
Values valueRot3 = computeOrientations(pose3Graph);
|
||||
|
||||
// Compute the full poses (1 GN iteration on full poses)
|
||||
return computePoses(pose3Graph, valueRot3);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess) {
|
||||
Values initialValues;
|
||||
|
||||
// get the orientation estimates from LAGO
|
||||
|
|
|
|||
|
|
@ -38,25 +38,11 @@ GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph);
|
|||
|
||||
GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
|
||||
|
||||
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess);
|
||||
GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot);
|
||||
|
||||
///** Linear factor graph with regularized orientation measurements */
|
||||
//GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
|
||||
// const std::vector<size_t>& spanningTreeIds,
|
||||
// const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
|
||||
// const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
|
||||
//
|
||||
///** LAGO: Return the orientations of the Pose2 in a generic factor graph */
|
||||
//GTSAM_EXPORT VectorValues initializeOrientations(
|
||||
// const NonlinearFactorGraph& graph, bool useOdometricPath = true);
|
||||
//
|
||||
///** Return the values for the Pose2 in a generic factor graph */
|
||||
//GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
|
||||
// bool useOdometricPath = true);
|
||||
//
|
||||
///** Only correct the orientation part in initialGuess */
|
||||
//GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
|
||||
// const Values& initialGuess);
|
||||
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
|
||||
|
||||
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess);
|
||||
|
||||
} // end of namespace lago
|
||||
} // end of namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue