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

Manhattan world example with covariances, in C++ and MATLAB

release/4.3a0
Frank Dellaert 2012-06-03 18:20:48 +00:00
parent 950bd8fcad
commit bd7724781d
7 changed files with 489 additions and 412 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

332
.cproject
View File

@ -311,6 +311,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -337,7 +345,6 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -345,7 +352,6 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -393,7 +399,6 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -401,7 +406,6 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -409,7 +413,6 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -425,20 +428,11 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -511,22 +505,6 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -543,6 +521,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -567,26 +561,18 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>all</buildTarget> <buildTarget>testValues.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>check</buildTarget> <buildTarget>testOrdering.run</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> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -671,18 +657,26 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testValues.run</buildTarget> <buildTarget>all</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testOrdering.run</buildTarget> <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> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -921,7 +915,6 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -929,7 +922,6 @@
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -937,7 +929,6 @@
</target> </target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget> <buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1049,7 +1040,6 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1505,6 +1495,7 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1544,6 +1535,7 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1551,6 +1543,7 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1748,10 +1741,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMwSPCG_advanced.run</buildTarget> <buildTarget>Pose2SLAMExample_graph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1766,6 +1759,7 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1787,6 +1781,102 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1988,7 +2078,6 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1996,7 +2085,6 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2004,7 +2092,6 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2012,7 +2099,6 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2066,98 +2152,42 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testRot3.run</buildTarget> <buildTarget>wrap.testSpirit.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testRot2.run</buildTarget> <buildTarget>wrap.testWrap.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testPose3.run</buildTarget> <buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>timeRot3.run</buildTarget> <buildTarget>wrap_gtsam</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testPose2.run</buildTarget> <buildTarget>wrap</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> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2201,46 +2231,6 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> </buildTargets>
</storageModule> </storageModule>
</cproject> </cproject>

53
examples/Pose2SLAMExample_graph.cpp Normal file
View File

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

64
examples/matlab/Pose2SLAMExample_graph.m
View File

@ -10,64 +10,30 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create graph container and add factors to it %% Initialize graph, initial estimate, and odometry noise
graph = pose2SLAMGraph; model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]);
initial = pose2SLAMValues; [graph,initial]=load2D('../Data/w100-odom.graph',model);
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); initial.print('Initial estimate:\n');
constraint=gtsamPose2; % identity
%% Add a Gaussian prior on pose x_1 %% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin 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 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 %% Plot Initial Estimate
figure(1);clf figure(1);clf
plot(initial.xs(),initial.ys(),'r-*'); axis equal plot(initial.xs(),initial.ys(),'g-*'); 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
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initial); 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 %% Plot Covariance Ellipses
% marginals = graph.marginals(result); marginals = graph.marginals(result);
% for i=0:result2.size()-1 for i=1:result.size()-1
% pose_i = result.pose(i); pose_i = result.pose(i);
% P_i=marginals.marginalCovariance(i); P{i}=marginals.marginalCovariance(i);
% covarianceEllipse([pose_i.x;pose_i.y],P_i,'g') covarianceEllipse([pose_i.x;pose_i.y],P{i},'b')
% end end
fprintf(1,'%.5f %.5f %.5f\n',P{99})

29
examples/matlab/load2D.m Normal file
View File

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

342
gtsam/slam/dataset.cpp
View File

@ -16,7 +16,6 @@
* @brief utility functions for loading datasets * @brief utility functions for loading datasets
*/ */
#include <fstream> #include <fstream>
#include <sstream> #include <sstream>
#include <cstdlib> #include <cstdlib>
@ -28,192 +27,219 @@ using namespace gtsam;
#define LINESIZE 81920 #define LINESIZE 81920
typedef boost::shared_ptr<pose2SLAM::Graph> sharedPose2Graph;
typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, const string& user_path) { pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset,
string path = user_path, set = dataset; const string& user_path) {
boost::optional<SharedDiagonal> null_model; string path = user_path, set = dataset;
boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3)); boost::optional<SharedDiagonal> null_model;
boost::optional<SharedDiagonal> small(noiseModel::Diagonal::Variances( boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3));
gtsam::Vector_(3, 0.0001, 0.0001, 0.0003))); 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 (path.empty())
if (set.empty()) set = string(getenv("DATASET")); 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")
if (set == "intel-gfs") return make_pair(path + "/Intel/intel.gfs.graph", null_model); return make_pair(path + "/Intel/intel.graph", null_model);
if (set == "Killian-gfs") return make_pair(path + "/Killian/Killian.gfs.graph", null_model); if (set == "intel-gfs")
if (set == "Killian") return make_pair(path + "/Killian/Killian.graph", small); return make_pair(path + "/Intel/intel.gfs.graph", null_model);
if (set == "Killian-noised") return make_pair(path + "/Killian/Killian-noised.graph", null_model); if (set == "Killian-gfs")
if (set == "3") return make_pair(path + "/TORO/w3-odom.graph", identity); return make_pair(path + "/Killian/Killian.gfs.graph", null_model);
if (set == "100") return make_pair(path + "/TORO/w100-odom.graph", identity); if (set == "Killian")
if (set == "10K") return make_pair(path + "/TORO/w10000-odom.graph", identity); return make_pair(path + "/Killian/Killian.graph", small);
if (set == "10K2") return make_pair(path + "/hogman/data/2D/w10000.graph", if (set == "Killian-noised")
noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05))); return make_pair(path + "/Killian/Killian-noised.graph", null_model);
if (set == "Eiffel100") return make_pair(path + "/TORO/w100-Eiffel.graph", identity); if (set == "3")
if (set == "Eiffel10K") return make_pair(path + "/TORO/w10000-Eiffel.graph", identity); return make_pair(path + "/TORO/w3-odom.graph", identity);
if (set == "olson") return make_pair(path + "/Olson/olson06icra.graph", null_model); if (set == "100")
if (set == "victoria") return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model); return make_pair(path + "/TORO/w100-odom.graph", identity);
if (set == "beijing") return make_pair(path + "/Beijing/beijingData_trips.graph", null_model); 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); 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);
} }
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 pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
while (is) { const string& filename, boost::optional<SharedDiagonal> model, int maxID,
is >> tag; bool addNoise, bool smart) {
cout << "Will try to read " << filename << endl;
if ((tag == "VERTEX2") || (tag == "VERTEX")) { ifstream is(filename.c_str());
int id; if (!is) {
double x, y, yaw; cout << "load2D: can not find the file!";
is >> id >> x >> y >> yaw; exit(-1);
// optional filter
if (maxID && id >= maxID) continue;
poses->insert(id, Pose2(x, y, yaw));
} }
is.ignore(LINESIZE, '\n');
}
is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg);
// load the factors pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values);
while (is) { pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph);
is >> tag;
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { string tag;
int id1, id2;
double x, y, yaw;
is >> id1 >> id2 >> x >> y >> yaw; // load the poses
Matrix m = eye(3); while (is) {
is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2); is >> tag;
m(2, 0) = m(0, 2);
m(2, 1) = m(1, 2);
m(1, 0) = m(0, 1);
// optional filter if ((tag == "VERTEX2") || (tag == "VERTEX")) {
if (maxID && (id1 >= maxID || id2 >= maxID)) continue; int id;
double x, y, yaw;
Pose2 l1Xl2(x, y, yaw); is >> id >> x >> y >> yaw;
// optional filter
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); if (maxID && id >= maxID)
if (!model) { continue;
Vector variances = Vector_(3,m(0,0),m(1,1),m(2,2)); poses->insert(id, Pose2(x, y, yaw));
model = noiseModel::Diagonal::Variances(variances, smart);
} }
is.ignore(LINESIZE, '\n');
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.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 " // load the factors
<< graph->nrFactors() << " factors" << endl; while (is) {
is >> tag;
return make_pair(graph, poses); if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
} int id1, id2;
double x, y, yaw;
/* ************************************************************************* */ is >> id1 >> id2 >> x >> y >> yaw;
void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDiagonal model, Matrix m = eye(3);
const string& filename) { 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 Pose2 l1Xl2(x, y, yaw);
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 // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
Matrix R = model->R(); if (!model) {
Matrix RR = trans(R)*R;//prod(trans(R),R); Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2));
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) { model = noiseModel::Diagonal::Variances(variances, smart);
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_); }
if (!factor) continue;
Pose2 pose = factor->measured().inverse(); if (addNoise)
stream << "EDGE2 " << factor->key2() << " " << factor->key1() l1Xl2 = l1Xl2.retract((*model)->sample());
<< " " << 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(); // 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(
bool load3D(const string& filename) { new pose2SLAM::Odometry(id1, id2, l1Xl2, *model));
ifstream is(filename.c_str()); graph->push_back(factor);
if (!is) return false; }
is.ignore(LINESIZE, '\n');
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;
} }
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]; void save2D(const pose2SLAM::Graph& graph, const Values& config,
is.getline(buf, LINESIZE); const SharedDiagonal model, const string& filename) {
istringstream ls(buf);
string tag;
ls >> tag;
if (tag == "EDGE3") { fstream stream(filename.c_str(), fstream::out);
int id1, id2;
double x, y, z, roll, pitch, yaw; // save poses
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config)
Matrix m = eye(6); {
for (int i = 0; i < 6; i++) const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
for (int j = i; j < 6; j++) stream << "VERTEX2 " << key_value.key << " " << pose.x() << " "
ls >> m(i, j); << 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;
}
} }

73
gtsam/slam/dataset.h
View File

@ -18,43 +18,52 @@
#pragma once #pragma once
#include <string>
#include <gtsam/slam/pose2SLAM.h> #include <gtsam/slam/pose2SLAM.h>
#include <string>
namespace gtsam namespace gtsam {
{ /**
/** * Construct dataset filename from short name
* Construct dataset filename from short name * Currently has "Killian" "intel.gfs", "10K", etc...
* Currently has "Killian" "intel.gfs", "10K", etc... * @param filename
* @param filename * @param optional dataset, if empty will try to getenv $DATASET
* @param optional dataset, if empty will try to getenv $DATASET * @param optional path, if empty will try to getenv $HOME
* @param optional path, if empty will try to getenv $HOME */
*/ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
dataset(const std::string& dataset = "", const std::string& path = ""); dataset(const std::string& dataset = "", const std::string& path = "");
/** /**
* Load TORO 2D Graph * Load TORO 2D Graph
* @param filename * @param dataset/model pair as constructed by [dataset]
* @param maxID, if non-zero cut out vertices >= maxID * @param maxID if non-zero cut out vertices >= maxID
* @param smart: try to reduce complexity of covariance to cheapest model * @param addNoise add noise to the edges
*/ * @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, std::pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
int maxID = 0, bool addNoise=false, bool smart=true); std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > load2D( int maxID = 0, bool addNoise = false, bool smart = true);
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
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, * Load TORO 2D Graph
const std::string& filename); * @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);
/** /** save 2d graph */
* Load TORO 3D Graph void save2D(const pose2SLAM::Graph& graph, const Values& config,
*/ const gtsam::SharedDiagonal model, const std::string& filename);
bool load3D(const std::string& filename);
/**
* Load TORO 3D Graph
*/
bool load3D(const std::string& filename);
} // namespace gtsam } // namespace gtsam

8
gtsam/slam/pose2SLAM.h
View File

@ -33,7 +33,9 @@ namespace pose2SLAM {
/// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
struct Values: public gtsam::Values { struct Values: public gtsam::Values {
/// Default constructor typedef boost::shared_ptr<Values> shared_ptr;
/// Default constructor
Values() {} Values() {}
/// Copy constructor /// Copy constructor
@ -75,7 +77,9 @@ namespace pose2SLAM {
/// Graph /// Graph
struct Graph: public NonlinearFactorGraph { struct Graph: public NonlinearFactorGraph {
/// Default constructor for a NonlinearFactorGraph typedef boost::shared_ptr<Graph> shared_ptr;
/// Default constructor for a NonlinearFactorGraph
Graph(){} Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph