Manhattan world example with covariances, in C++ and MATLAB
Frank Dellaert
parent
950bd8fcad
commit
bd7724781d
332
.cproject
332
.cproject
|
|
@ -311,6 +311,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -337,7 +345,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>
|
||||
|
|
@ -345,7 +352,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>
|
||||
|
|
@ -393,7 +399,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>
|
||||
|
|
@ -401,7 +406,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>
|
||||
|
|
@ -409,7 +413,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>
|
||||
|
|
@ -425,20 +428,11 @@
|
|||
</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>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -511,22 +505,6 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -543,6 +521,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -567,26 +561,18 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testValues.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -671,18 +657,26 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testValues.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -921,7 +915,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>
|
||||
|
|
@ -929,7 +922,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>
|
||||
|
|
@ -937,7 +929,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>
|
||||
|
|
@ -1049,7 +1040,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>
|
||||
|
|
@ -1505,6 +1495,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>
|
||||
|
|
@ -1544,6 +1535,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>
|
||||
|
|
@ -1551,6 +1543,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>
|
||||
|
|
@ -1748,10 +1741,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="Pose2SLAMwSPCG_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="Pose2SLAMExample_graph.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>Pose2SLAMwSPCG_advanced.run</buildTarget>
|
||||
<buildTarget>Pose2SLAMExample_graph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1766,6 +1759,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>
|
||||
|
|
@ -1787,6 +1781,102 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>timeRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCal3_S2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testHomography2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1988,7 +2078,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>
|
||||
|
|
@ -1996,7 +2085,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>
|
||||
|
|
@ -2004,7 +2092,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>
|
||||
|
|
@ -2012,7 +2099,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>
|
||||
|
|
@ -2066,98 +2152,42 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap.testSpirit.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="wrap.testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap.testWrap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="wrap_gtsam" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>timeRot3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap_gtsam</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCal3_S2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testHomography2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2201,46 +2231,6 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap.testSpirit.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap.testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap.testWrap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap_gtsam" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap_gtsam</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cproject>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,53 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Pose2SLAMExample_graph->cpp
|
||||
* @brief Read graph from file and perform GraphSLAM
|
||||
* @date June 3, 2012
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
// Read File and create graph and initial estimate
|
||||
// we are in build/examples, data is in examples/Data
|
||||
pose2SLAM::Graph::shared_ptr graph ;
|
||||
pose2SLAM::Values::shared_ptr initial;
|
||||
SharedDiagonal model(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0));
|
||||
boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model);
|
||||
initial->print("Initial estimate:\n");
|
||||
|
||||
// Add a Gaussian prior on first poses
|
||||
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
|
||||
SharedDiagonal priorNoise(Vector_(3, 0.01, 0.01, 0.01));
|
||||
graph->addPrior(0, priorMean, priorNoise);
|
||||
|
||||
// Single Step Optimization using Levenberg-Marquardt
|
||||
pose2SLAM::Values result = graph->optimize(*initial);
|
||||
result.print("\nFinal result:\n");
|
||||
|
||||
// Plot the covariance of the last pose
|
||||
Marginals marginals(*graph, result);
|
||||
cout.precision(2);
|
||||
cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -10,64 +10,30 @@
|
|||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%% Create graph container and add factors to it
|
||||
graph = pose2SLAMGraph;
|
||||
initial = pose2SLAMValues;
|
||||
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
|
||||
constraint=gtsamPose2; % identity
|
||||
%% Initialize graph, initial estimate, and odometry noise
|
||||
model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]);
|
||||
[graph,initial]=load2D('../Data/w100-odom.graph',model);
|
||||
initial.print('Initial estimate:\n');
|
||||
|
||||
%% Add a Gaussian prior on pose x_1
|
||||
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
|
||||
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
|
||||
priorNoise = gtsamSharedNoiseModel_Sigmas([0.01; 0.01; 0.01]);
|
||||
graph.addPrior(0, priorMean, priorNoise); % add directly to graph
|
||||
|
||||
%% Read File and create graph and initial estimate
|
||||
fid = fopen('../Data/w100-odom.graph');
|
||||
if fid < 0
|
||||
error('Cannot open the file ');
|
||||
end
|
||||
|
||||
columns=textscan(fid,'%s','delimiter','\n');
|
||||
fclose(fid);
|
||||
lines=columns{1};
|
||||
n=size(lines,1);
|
||||
|
||||
for i=1:n
|
||||
line_i=lines{i};
|
||||
if strcmp('VERTEX2',line_i(1:7))
|
||||
v = textscan(line_i,'%s %d %f %f %f',1);
|
||||
initial.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5}));
|
||||
elseif strcmp('EDGE2',line_i(1:5))
|
||||
e = textscan(line_i,'%s %d %d %f %f %f',1);
|
||||
graph.addOdometry(e{2}, e{3}, gtsamPose2(e{4}, e{5}, e{6}), odometryNoise);
|
||||
end
|
||||
end
|
||||
|
||||
%% Plot Initial Estimate
|
||||
figure(1);clf
|
||||
plot(initial.xs(),initial.ys(),'r-*'); axis equal
|
||||
|
||||
addEquivalences=0;
|
||||
if addEquivalences
|
||||
%% Add equivalence constraints
|
||||
for i=1:n
|
||||
line_i=cell2mat(lines(i));
|
||||
if strcmp('EQUIV',line_i(1:5))
|
||||
equiv = textscan(line_i,'%s %d %d',1);
|
||||
graph.addOdometry(e{2}, e{3}, constraint, odometryNoise);
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
plot(initial.xs(),initial.ys(),'g-*'); axis equal
|
||||
|
||||
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
||||
result = graph.optimize(initial);
|
||||
hold on; plot(result.xs(),result.ys(),'g-*')
|
||||
hold on; plot(result.xs(),result.ys(),'b-*')
|
||||
result.print('\nFinal result:\n');
|
||||
|
||||
%% Plot Covariance Ellipses
|
||||
% marginals = graph.marginals(result);
|
||||
% for i=0:result2.size()-1
|
||||
% pose_i = result.pose(i);
|
||||
% P_i=marginals.marginalCovariance(i);
|
||||
% covarianceEllipse([pose_i.x;pose_i.y],P_i,'g')
|
||||
% end
|
||||
marginals = graph.marginals(result);
|
||||
for i=1:result.size()-1
|
||||
pose_i = result.pose(i);
|
||||
P{i}=marginals.marginalCovariance(i);
|
||||
covarianceEllipse([pose_i.x;pose_i.y],P{i},'b')
|
||||
end
|
||||
fprintf(1,'%.5f %.5f %.5f\n',P{99})
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
function [graph,initial] = load2D(filename,model)
|
||||
% load2D: read TORO pose graph
|
||||
% cannot read noise model from file yet, uses specified model
|
||||
|
||||
fid = fopen(filename);
|
||||
if fid < 0
|
||||
error(['load2D: Cannot open file ' filename]);
|
||||
end
|
||||
|
||||
% scan all lines into a cell array
|
||||
columns=textscan(fid,'%s','delimiter','\n');
|
||||
fclose(fid);
|
||||
lines=columns{1};
|
||||
|
||||
% loop over lines and add vertices
|
||||
graph = pose2SLAMGraph;
|
||||
initial = pose2SLAMValues;
|
||||
n=size(lines,1);
|
||||
for i=1:n
|
||||
line_i=lines{i};
|
||||
if strcmp('VERTEX2',line_i(1:7))
|
||||
v = textscan(line_i,'%s %d %f %f %f',1);
|
||||
initial.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5}));
|
||||
elseif strcmp('EDGE2',line_i(1:5))
|
||||
e = textscan(line_i,'%s %d %d %f %f %f',1);
|
||||
graph.addOdometry(e{2}, e{3}, gtsamPose2(e{4}, e{5}, e{6}), model);
|
||||
end
|
||||
end
|
||||
|
||||
|
|
@ -16,7 +16,6 @@
|
|||
* @brief utility functions for loading datasets
|
||||
*/
|
||||
|
||||
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <cstdlib>
|
||||
|
|
@ -28,192 +27,219 @@ using namespace gtsam;
|
|||
|
||||
#define LINESIZE 81920
|
||||
|
||||
typedef boost::shared_ptr<pose2SLAM::Graph> sharedPose2Graph;
|
||||
typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, const string& user_path) {
|
||||
string path = user_path, set = dataset;
|
||||
boost::optional<SharedDiagonal> null_model;
|
||||
boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3));
|
||||
boost::optional<SharedDiagonal> small(noiseModel::Diagonal::Variances(
|
||||
gtsam::Vector_(3, 0.0001, 0.0001, 0.0003)));
|
||||
/* ************************************************************************* */
|
||||
pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset,
|
||||
const string& user_path) {
|
||||
string path = user_path, set = dataset;
|
||||
boost::optional<SharedDiagonal> null_model;
|
||||
boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3));
|
||||
boost::optional<SharedDiagonal> small(
|
||||
noiseModel::Diagonal::Variances(
|
||||
gtsam::Vector_(3, 0.0001, 0.0001, 0.0003)));
|
||||
|
||||
if (path.empty()) path = string(getenv("HOME")) + "/data";
|
||||
if (set.empty()) set = string(getenv("DATASET"));
|
||||
if (path.empty())
|
||||
path = string(getenv("HOME")) + "/data";
|
||||
if (set.empty())
|
||||
set = string(getenv("DATASET"));
|
||||
|
||||
if (set == "intel") return make_pair(path + "/Intel/intel.graph", null_model);
|
||||
if (set == "intel-gfs") return make_pair(path + "/Intel/intel.gfs.graph", null_model);
|
||||
if (set == "Killian-gfs") return make_pair(path + "/Killian/Killian.gfs.graph", null_model);
|
||||
if (set == "Killian") return make_pair(path + "/Killian/Killian.graph", small);
|
||||
if (set == "Killian-noised") return make_pair(path + "/Killian/Killian-noised.graph", null_model);
|
||||
if (set == "3") return make_pair(path + "/TORO/w3-odom.graph", identity);
|
||||
if (set == "100") return make_pair(path + "/TORO/w100-odom.graph", identity);
|
||||
if (set == "10K") return make_pair(path + "/TORO/w10000-odom.graph", identity);
|
||||
if (set == "10K2") return make_pair(path + "/hogman/data/2D/w10000.graph",
|
||||
noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05)));
|
||||
if (set == "Eiffel100") return make_pair(path + "/TORO/w100-Eiffel.graph", identity);
|
||||
if (set == "Eiffel10K") return make_pair(path + "/TORO/w10000-Eiffel.graph", identity);
|
||||
if (set == "olson") return make_pair(path + "/Olson/olson06icra.graph", null_model);
|
||||
if (set == "victoria") return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model);
|
||||
if (set == "beijing") return make_pair(path + "/Beijing/beijingData_trips.graph", null_model);
|
||||
if (set == "intel")
|
||||
return make_pair(path + "/Intel/intel.graph", null_model);
|
||||
if (set == "intel-gfs")
|
||||
return make_pair(path + "/Intel/intel.gfs.graph", null_model);
|
||||
if (set == "Killian-gfs")
|
||||
return make_pair(path + "/Killian/Killian.gfs.graph", null_model);
|
||||
if (set == "Killian")
|
||||
return make_pair(path + "/Killian/Killian.graph", small);
|
||||
if (set == "Killian-noised")
|
||||
return make_pair(path + "/Killian/Killian-noised.graph", null_model);
|
||||
if (set == "3")
|
||||
return make_pair(path + "/TORO/w3-odom.graph", identity);
|
||||
if (set == "100")
|
||||
return make_pair(path + "/TORO/w100-odom.graph", identity);
|
||||
if (set == "10K")
|
||||
return make_pair(path + "/TORO/w10000-odom.graph", identity);
|
||||
if (set == "10K2")
|
||||
return make_pair(path + "/hogman/data/2D/w10000.graph",
|
||||
noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05)));
|
||||
if (set == "Eiffel100")
|
||||
return make_pair(path + "/TORO/w100-Eiffel.graph", identity);
|
||||
if (set == "Eiffel10K")
|
||||
return make_pair(path + "/TORO/w10000-Eiffel.graph", identity);
|
||||
if (set == "olson")
|
||||
return make_pair(path + "/Olson/olson06icra.graph", null_model);
|
||||
if (set == "victoria")
|
||||
return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model);
|
||||
if (set == "beijing")
|
||||
return make_pair(path + "/Beijing/beijingData_trips.graph", null_model);
|
||||
|
||||
return make_pair("unknown", null_model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
pair<sharedPose2Graph, Values::shared_ptr> load2D(
|
||||
pair<string, boost::optional<SharedDiagonal> > dataset,
|
||||
int maxID, bool addNoise, bool smart) {
|
||||
return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
|
||||
}
|
||||
|
||||
pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
|
||||
boost::optional<SharedDiagonal> model, int maxID, bool addNoise, bool smart) {
|
||||
cout << "Will try to read " << filename << endl;
|
||||
ifstream is(filename.c_str());
|
||||
if (!is) {
|
||||
cout << "load2D: can not find the file!";
|
||||
exit(-1);
|
||||
return make_pair("unknown", null_model);
|
||||
}
|
||||
|
||||
Values::shared_ptr poses(new Values);
|
||||
sharedPose2Graph graph(new pose2SLAM::Graph);
|
||||
/* ************************************************************************* */
|
||||
|
||||
string tag;
|
||||
pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
|
||||
pair<string, boost::optional<SharedDiagonal> > dataset,
|
||||
int maxID, bool addNoise, bool smart) {
|
||||
return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
|
||||
}
|
||||
|
||||
// load the poses
|
||||
while (is) {
|
||||
is >> tag;
|
||||
|
||||
if ((tag == "VERTEX2") || (tag == "VERTEX")) {
|
||||
int id;
|
||||
double x, y, yaw;
|
||||
is >> id >> x >> y >> yaw;
|
||||
// optional filter
|
||||
if (maxID && id >= maxID) continue;
|
||||
poses->insert(id, Pose2(x, y, yaw));
|
||||
pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
|
||||
const string& filename, boost::optional<SharedDiagonal> model, int maxID,
|
||||
bool addNoise, bool smart) {
|
||||
cout << "Will try to read " << filename << endl;
|
||||
ifstream is(filename.c_str());
|
||||
if (!is) {
|
||||
cout << "load2D: can not find the file!";
|
||||
exit(-1);
|
||||
}
|
||||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
is.seekg(0, ios::beg);
|
||||
|
||||
// load the factors
|
||||
while (is) {
|
||||
is >> tag;
|
||||
pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values);
|
||||
pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph);
|
||||
|
||||
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
|
||||
int id1, id2;
|
||||
double x, y, yaw;
|
||||
string tag;
|
||||
|
||||
is >> id1 >> id2 >> x >> y >> yaw;
|
||||
Matrix m = eye(3);
|
||||
is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2);
|
||||
m(2, 0) = m(0, 2);
|
||||
m(2, 1) = m(1, 2);
|
||||
m(1, 0) = m(0, 1);
|
||||
// load the poses
|
||||
while (is) {
|
||||
is >> tag;
|
||||
|
||||
// optional filter
|
||||
if (maxID && (id1 >= maxID || id2 >= maxID)) continue;
|
||||
|
||||
Pose2 l1Xl2(x, y, yaw);
|
||||
|
||||
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
|
||||
if (!model) {
|
||||
Vector variances = Vector_(3,m(0,0),m(1,1),m(2,2));
|
||||
model = noiseModel::Diagonal::Variances(variances, smart);
|
||||
if ((tag == "VERTEX2") || (tag == "VERTEX")) {
|
||||
int id;
|
||||
double x, y, yaw;
|
||||
is >> id >> x >> y >> yaw;
|
||||
// optional filter
|
||||
if (maxID && id >= maxID)
|
||||
continue;
|
||||
poses->insert(id, Pose2(x, y, yaw));
|
||||
}
|
||||
|
||||
if (addNoise)
|
||||
l1Xl2 = l1Xl2.retract((*model)->sample());
|
||||
|
||||
// Insert vertices if pure odometry file
|
||||
if (!poses->exists(id1)) poses->insert(id1, Pose2());
|
||||
if (!poses->exists(id2)) poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
|
||||
|
||||
pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
|
||||
graph->push_back(factor);
|
||||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
is.seekg(0, ios::beg);
|
||||
|
||||
cout << "load2D read a graph file with " << poses->size() << " vertices and "
|
||||
<< graph->nrFactors() << " factors" << endl;
|
||||
// load the factors
|
||||
while (is) {
|
||||
is >> tag;
|
||||
|
||||
return make_pair(graph, poses);
|
||||
}
|
||||
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
|
||||
int id1, id2;
|
||||
double x, y, yaw;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDiagonal model,
|
||||
const string& filename) {
|
||||
is >> id1 >> id2 >> x >> y >> yaw;
|
||||
Matrix m = eye(3);
|
||||
is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2);
|
||||
m(2, 0) = m(0, 2);
|
||||
m(2, 1) = m(1, 2);
|
||||
m(1, 0) = m(0, 1);
|
||||
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
// optional filter
|
||||
if (maxID && (id1 >= maxID || id2 >= maxID))
|
||||
continue;
|
||||
|
||||
// save poses
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
|
||||
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
|
||||
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
|
||||
}
|
||||
Pose2 l1Xl2(x, y, yaw);
|
||||
|
||||
// save edges
|
||||
Matrix R = model->R();
|
||||
Matrix RR = trans(R)*R;//prod(trans(R),R);
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
|
||||
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
|
||||
if (!factor) continue;
|
||||
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
|
||||
if (!model) {
|
||||
Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2));
|
||||
model = noiseModel::Diagonal::Variances(variances, smart);
|
||||
}
|
||||
|
||||
Pose2 pose = factor->measured().inverse();
|
||||
stream << "EDGE2 " << factor->key2() << " " << factor->key1()
|
||||
<< " " << pose.x() << " " << pose.y() << " " << pose.theta()
|
||||
<< " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
|
||||
<< " " << RR(0, 2) << " " << RR(1, 2) << endl;
|
||||
}
|
||||
if (addNoise)
|
||||
l1Xl2 = l1Xl2.retract((*model)->sample());
|
||||
|
||||
stream.close();
|
||||
}
|
||||
// Insert vertices if pure odometry file
|
||||
if (!poses->exists(id1))
|
||||
poses->insert(id1, Pose2());
|
||||
if (!poses->exists(id2))
|
||||
poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool load3D(const string& filename) {
|
||||
ifstream is(filename.c_str());
|
||||
if (!is) return false;
|
||||
|
||||
while (is) {
|
||||
char buf[LINESIZE];
|
||||
is.getline(buf, LINESIZE);
|
||||
istringstream ls(buf);
|
||||
string tag;
|
||||
ls >> tag;
|
||||
|
||||
if (tag == "VERTEX3") {
|
||||
int id;
|
||||
double x, y, z, roll, pitch, yaw;
|
||||
ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
|
||||
pose2SLAM::Graph::sharedFactor factor(
|
||||
new pose2SLAM::Odometry(id1, id2, l1Xl2, *model));
|
||||
graph->push_back(factor);
|
||||
}
|
||||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
|
||||
cout << "load2D read a graph file with " << poses->size()
|
||||
<< " vertices and " << graph->nrFactors() << " factors" << endl;
|
||||
|
||||
return make_pair(graph, poses);
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
is.seekg(0, ios::beg);
|
||||
|
||||
while (is) {
|
||||
char buf[LINESIZE];
|
||||
is.getline(buf, LINESIZE);
|
||||
istringstream ls(buf);
|
||||
string tag;
|
||||
ls >> tag;
|
||||
/* ************************************************************************* */
|
||||
void save2D(const pose2SLAM::Graph& graph, const Values& config,
|
||||
const SharedDiagonal model, const string& filename) {
|
||||
|
||||
if (tag == "EDGE3") {
|
||||
int id1, id2;
|
||||
double x, y, z, roll, pitch, yaw;
|
||||
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
|
||||
Matrix m = eye(6);
|
||||
for (int i = 0; i < 6; i++)
|
||||
for (int j = i; j < 6; j++)
|
||||
ls >> m(i, j);
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
|
||||
// save poses
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config)
|
||||
{
|
||||
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
|
||||
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " "
|
||||
<< pose.y() << " " << pose.theta() << endl;
|
||||
}
|
||||
|
||||
// save edges
|
||||
Matrix R = model->R();
|
||||
Matrix RR = trans(R) * R; //prod(trans(R),R);
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
|
||||
{
|
||||
boost::shared_ptr<pose2SLAM::Odometry> factor =
|
||||
boost::dynamic_pointer_cast<pose2SLAM::Odometry>(factor_);
|
||||
if (!factor)
|
||||
continue;
|
||||
|
||||
Pose2 pose = factor->measured().inverse();
|
||||
stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
|
||||
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
|
||||
<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " "
|
||||
<< RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl;
|
||||
}
|
||||
|
||||
stream.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool load3D(const string& filename) {
|
||||
ifstream is(filename.c_str());
|
||||
if (!is)
|
||||
return false;
|
||||
|
||||
while (is) {
|
||||
char buf[LINESIZE];
|
||||
is.getline(buf, LINESIZE);
|
||||
istringstream ls(buf);
|
||||
string tag;
|
||||
ls >> tag;
|
||||
|
||||
if (tag == "VERTEX3") {
|
||||
int id;
|
||||
double x, y, z, roll, pitch, yaw;
|
||||
ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
|
||||
}
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
is.seekg(0, ios::beg);
|
||||
|
||||
while (is) {
|
||||
char buf[LINESIZE];
|
||||
is.getline(buf, LINESIZE);
|
||||
istringstream ls(buf);
|
||||
string tag;
|
||||
ls >> tag;
|
||||
|
||||
if (tag == "EDGE3") {
|
||||
int id1, id2;
|
||||
double x, y, z, roll, pitch, yaw;
|
||||
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
|
||||
Matrix m = eye(6);
|
||||
for (int i = 0; i < 6; i++)
|
||||
for (int j = i; j < 6; j++)
|
||||
ls >> m(i, j);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,43 +18,52 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <string>
|
||||
|
||||
namespace gtsam
|
||||
{
|
||||
/**
|
||||
* Construct dataset filename from short name
|
||||
* Currently has "Killian" "intel.gfs", "10K", etc...
|
||||
* @param filename
|
||||
* @param optional dataset, if empty will try to getenv $DATASET
|
||||
* @param optional path, if empty will try to getenv $HOME
|
||||
*/
|
||||
std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
|
||||
namespace gtsam {
|
||||
/**
|
||||
* Construct dataset filename from short name
|
||||
* Currently has "Killian" "intel.gfs", "10K", etc...
|
||||
* @param filename
|
||||
* @param optional dataset, if empty will try to getenv $DATASET
|
||||
* @param optional path, if empty will try to getenv $HOME
|
||||
*/
|
||||
std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
|
||||
dataset(const std::string& dataset = "", const std::string& path = "");
|
||||
|
||||
/**
|
||||
* Load TORO 2D Graph
|
||||
* @param filename
|
||||
* @param maxID, if non-zero cut out vertices >= maxID
|
||||
* @param smart: try to reduce complexity of covariance to cheapest model
|
||||
*/
|
||||
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > load2D(
|
||||
std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
|
||||
int maxID = 0, bool addNoise=false, bool smart=true);
|
||||
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > load2D(
|
||||
const std::string& filename,
|
||||
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
|
||||
int maxID = 0, bool addNoise=false, bool smart=true);
|
||||
/**
|
||||
* Load TORO 2D Graph
|
||||
* @param dataset/model pair as constructed by [dataset]
|
||||
* @param maxID if non-zero cut out vertices >= maxID
|
||||
* @param addNoise add noise to the edges
|
||||
* @param smart try to reduce complexity of covariance to cheapest model
|
||||
*/
|
||||
std::pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
|
||||
std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
|
||||
int maxID = 0, bool addNoise = false, bool smart = true);
|
||||
|
||||
/** save 2d graph */
|
||||
void save2D(const pose2SLAM::Graph& graph, const Values& config, const gtsam::SharedDiagonal model,
|
||||
const std::string& filename);
|
||||
/**
|
||||
* Load TORO 2D Graph
|
||||
* @param filename
|
||||
* @param model optional noise model to use instead of one specified by file
|
||||
* @param maxID if non-zero cut out vertices >= maxID
|
||||
* @param addNoise add noise to the edges
|
||||
* @param smart try to reduce complexity of covariance to cheapest model
|
||||
*/
|
||||
std::pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
|
||||
const std::string& filename,
|
||||
boost::optional<gtsam::SharedDiagonal> model = boost::optional<
|
||||
gtsam::SharedDiagonal>(), int maxID = 0, bool addNoise = false,
|
||||
bool smart = true);
|
||||
|
||||
/**
|
||||
* Load TORO 3D Graph
|
||||
*/
|
||||
bool load3D(const std::string& filename);
|
||||
/** save 2d graph */
|
||||
void save2D(const pose2SLAM::Graph& graph, const Values& config,
|
||||
const gtsam::SharedDiagonal model, const std::string& filename);
|
||||
|
||||
/**
|
||||
* Load TORO 3D Graph
|
||||
*/
|
||||
bool load3D(const std::string& filename);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -33,7 +33,9 @@ namespace pose2SLAM {
|
|||
/// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
|
||||
struct Values: public gtsam::Values {
|
||||
|
||||
/// Default constructor
|
||||
typedef boost::shared_ptr<Values> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
Values() {}
|
||||
|
||||
/// Copy constructor
|
||||
|
|
@ -75,7 +77,9 @@ namespace pose2SLAM {
|
|||
/// Graph
|
||||
struct Graph: public NonlinearFactorGraph {
|
||||
|
||||
/// Default constructor for a NonlinearFactorGraph
|
||||
typedef boost::shared_ptr<Graph> shared_ptr;
|
||||
|
||||
/// Default constructor for a NonlinearFactorGraph
|
||||
Graph(){}
|
||||
|
||||
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
|
||||
|
|
|
|||
Loading…
Reference in New Issue