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

Merge branch 'release/2.1.0' 

Conflicts:
	examples/vSLAMexample/vISAMexample.cpp
release/4.3a0
cbeall3 2015-03-06 23:59:33 -05:00
parent a0ec8ee14e d2432bf19f
commit e39dd5901c
1273 changed files with 175364 additions and 32746 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

680
.cproject
View File

@ -35,10 +35,6 @@
<tool id="cdt.managedbuild.tool.gnu.archiver.macosx.base.217086069" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.macosx.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.1629258328" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base">
<option id="gnu.cpp.compiler.option.include.paths.1552452888" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/gtsam}&quot;"/>
<listOptionValue builtIn="false" value="/usr/include/c++/4.6.1"/>
<listOptionValue builtIn="false" value="/usr/include/c++/4.6"/>
<listOptionValue builtIn="false" value="/usr/include/c++/4.6/backward"/>
<listOptionValue builtIn="false" value="&quot;${ProjDirPath}&quot;"/>
</option>
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
@ -285,6 +281,8 @@
</profile>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets>
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@ -439,156 +437,18 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoseRTV.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testIMUSystem.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianISAM.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testIterative.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testIterative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearEquality.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearEquality.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearOptimizer.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSubgraphPreconditioner.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSubgraphPreconditioner.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTupleConfig.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testTupleConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="tests" 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="testGaussianJunctionTree" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianJunctionTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerialization.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSerialization.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testIMUSystem.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -745,34 +605,82 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGeneralSFMFactor.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPlanarSLAM.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testPlanarSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2SLAM.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3SLAM.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose3SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2DOriented.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVisualSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVisualSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testProjectionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerializationSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -793,6 +701,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -865,6 +781,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactor3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoseTranslationPrior.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoseTranslationPrior.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoseRotationPrior.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoseRotationPrior.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -873,6 +813,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteMarginals.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDiscreteMarginals.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInference.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -881,6 +837,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthCamera3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -897,22 +861,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeGaussianFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -921,42 +869,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTupleConfig.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testMarginals.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testTupleConfig.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testMarginals.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerialization.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussianISAM2.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSerialization.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianISAM2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInference.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testSymbolicFactorGraphB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testSymbolicFactorGraphB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -969,10 +901,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGradientDescentOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testGradientDescentOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -985,14 +917,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAMwSPCG.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2SLAMwSPCG.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1017,14 +941,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests.testBoundingConstraint.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>tests.testBoundingConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearEquality.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1049,6 +965,118 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactor.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTreeB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianJunctionTreeB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDoglegOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDoglegOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testIterative.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testIterative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSubgraphSolver.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSubgraphSolver.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraphB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianFactorGraphB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1185,6 +1213,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearContainerFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testLinearContainerFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1513,6 +1549,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1742,18 +1786,18 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CameraResectioning" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="CameraResectioning.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>CameraResectioning</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>CameraResectioning.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="PlanarSLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="PlanarSLAMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>PlanarSLAMExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1782,26 +1826,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="PlanarSLAMSelfContained_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="Pose2SLAMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExample_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>Pose2SLAMExample_advanced.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>Pose2SLAMExample_easy.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1822,6 +1850,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="LocalizationExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>LocalizationExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="OdometryExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>OdometryExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1855,10 +1899,10 @@
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildArguments>-j1</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@ -1869,12 +1913,12 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="check j1" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildArguments>-j1</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@ -2092,6 +2136,126 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.discrete_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.discrete_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.base_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.base_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.dynamics_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.dynamics_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.slam_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.slam_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_build" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_build</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_unstable_build" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_unstable_build</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap" path="" 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>
<target name="check.nonlinear_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.nonlinear_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_distclean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_unstable_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_unstable_distclean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="doc" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>doc</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="doc_clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>doc_clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<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>
@ -2227,18 +2391,18 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap.testSpirit.run</buildTarget>
<buildTarget>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">
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap.testWrap.run</buildTarget>
<buildTarget>testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -2251,14 +2415,6 @@
<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>

6
.settings/org.eclipse.cdt.core.prefs
View File

@ -1,6 +0,0 @@
#Wed Oct 06 11:57:41 EDT 2010
eclipse.preferences.version=1
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/append=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/appendContributed=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true

165
CMakeLists.txt
View File

@ -1,16 +1,26 @@
project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6)
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 2)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_MINOR 1)
set (GTSAM_VERSION_PATCH 0)
# Set the default install path to home
#set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library")
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
include(GtsamMakeConfigFile)
# Record the root dir for gtsam - needed during external builds, e.g., ROS
set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR})
message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
# Use macros for creating tests/timing scripts
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake")
include(GtsamTesting)
include(GtsamPrinting)
@ -26,40 +36,46 @@ else()
set(GTSAM_UNSTABLE_AVAILABLE 0)
endif()
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
# Check build types
if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
endif()
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
if( NOT cmake_build_type_tolower STREQUAL ""
AND NOT cmake_build_type_tolower STREQUAL "none"
AND NOT cmake_build_type_tolower STREQUAL "debug"
AND NOT cmake_build_type_tolower STREQUAL "release"
AND NOT cmake_build_type_tolower STREQUAL "timing"
AND NOT cmake_build_type_tolower STREQUAL "profiling"
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).")
endif()
# Configurable Options
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON)
#option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON) # These do not currently work
option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON)
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" OFF)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
endif()
if(NOT MSVC)
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON)
option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON)
else()
set(GTSAM_BUILD_STATIC_LIBRARY ON)
endif()
option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON)
option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF)
option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" ON)
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON)
option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON)
option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON)
option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON)
if(NOT MSVC)
option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" ON)
endif()
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility for wrapping other libraries" ON)
set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/toolbox")
set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
set(MEX_COMMAND "mex" CACHE FILEPATH "Command to use for executing mex (if on path, 'mex' will work)")
# Check / set dependent variables for MATLAB wrapper
set(GTSAM_WRAP_HEADER_PATH "${PROJECT_SOURCE_DIR}/wrap")
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/borg/toolbox")
endif()
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
@ -72,18 +88,14 @@ endif()
# Add the Quaternion Build Flag if requested
if (GTSAM_USE_QUATERNIONS)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
endif(GTSAM_USE_QUATERNIONS)
# Avoid building non-installed exes and unit tests when installing
# FIXME: breaks generation and install of matlab toolbox
# FIXME: can't add install dependencies, so libraries never get built
#set(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY TRUE)
# Alternative version to keep tests from building during make install
# Use the EXCLUDE_FROM_ALL property on test executables
option(GTSAM_ENABLE_INSTALL_TEST_FIX "Enable/Disable fix to remove dependency of tests on 'all' target" ON)
# Flags to determine whether tests and examples are build during 'make install'
# Note that these remove the targets from the 'all'
option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON)
option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables building examples during install" OFF)
# Pull in infrastructure
if (GTSAM_BUILD_TESTS)
@ -93,7 +105,11 @@ if (GTSAM_BUILD_TESTS)
endif()
# Find boost
find_package(Boost 1.40 COMPONENTS serialization REQUIRED)
if(CYGWIN OR MSVC OR WIN32)
set(Boost_USE_STATIC_LIBS 1)
endif()
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread date_time regex REQUIRED)
set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
# General build settings
include_directories(
@ -107,28 +123,45 @@ link_directories(${Boost_LIBRARY_DIRS})
# Build CppUnitLite
add_subdirectory(CppUnitLite)
# Build wrap
if (GTSAM_BUILD_WRAP)
add_subdirectory(wrap)
endif(GTSAM_BUILD_WRAP)
# Build GTSAM library
add_subdirectory(gtsam)
# Build Tests
add_subdirectory(tests)
# Build wrap
if (GTSAM_BUILD_WRAP)
add_subdirectory(wrap)
endif(GTSAM_BUILD_WRAP)
# Build examples
if (GTSAM_BUILD_EXAMPLES)
add_subdirectory(examples)
endif(GTSAM_BUILD_EXAMPLES)
# Matlab toolbox
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
add_subdirectory(matlab)
endif()
# Build gtsam_unstable
if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable)
endif(GTSAM_BUILD_UNSTABLE)
# Set up CPack
# Install config and export files
GtsamMakeConfigFile(GTSAM)
export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
# Check for doxygen availability - optional dependency
find_package(Doxygen)
# Doxygen documentation - enabling options in subfolder
if (DOXYGEN_FOUND)
add_subdirectory(doc)
endif()
## Set up CPack
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")
set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu")
@ -138,30 +171,39 @@ set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR})
set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH})
set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}")
set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory
set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makedoc.sh$;/gtsam_unstable/")
#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory
#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error
set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$")
set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/")
set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/")
set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs
# Record the root dir for gtsam - needed during external builds, e.g., ROS
set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR})
message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
# Deb-package specific cpack
set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
# print configuration variables
message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================")
message(STATUS "Build flags ")
print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ")
#print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ")
print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ")
print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ")
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ")
print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries ")
if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
if (DOXYGEN_FOUND)
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ")
endif()
print_config_flag(${GTSAM_ENABLE_INSTALL_TEST_FIX} "Tests excluded from all target ")
if(NOT MSVC)
print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ")
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ")
print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries ")
endif()
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build-type in library name ")
if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
endif()
print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "Tests excluded from all/install target ")
print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "Examples excluded from all/install target ")
string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper)
message(STATUS " Build type : ${CMAKE_BUILD_TYPE}")
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}")
@ -176,8 +218,7 @@ print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default R
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")
print_config_flag(${GTSAM_INSTALL_MATLAB_EXAMPLES} "Install matlab examples ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TESTS} "Install matlab tests ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ")
message(STATUS "===============================================================")

8
CppUnitLite/CMakeLists.txt
View File

@ -3,10 +3,14 @@
file(GLOB cppunitlite_headers "*.h")
file(GLOB cppunitlite_src "*.cpp")
add_library(CppUnitLite STATIC ${cppunitlite_src})
add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON)
if (GTSAM_INSTALL_CPPUNITLITE)
install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite)
install(TARGETS CppUnitLite ARCHIVE DESTINATION lib)
install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
endif(GTSAM_INSTALL_CPPUNITLITE)

71
CppUnitLite/Failure.cpp
View File

@ -1,71 +0,0 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#include "Failure.h"
#include <stdio.h>
#include <string.h>
Failure::Failure (const SimpleString& theTestName,
const SimpleString& theFileName,
long theLineNumber,
const SimpleString& theCondition)
: message (theCondition),
testName (theTestName),
fileName (theFileName),
lineNumber (theLineNumber)
{
}
Failure::Failure (const SimpleString& theTestName,
const SimpleString& theFileName,
const SimpleString& theCondition)
: message (theCondition),
testName (theTestName),
fileName (theFileName),
lineNumber (-1)
{
}
Failure::Failure (const SimpleString& theTestName,
const SimpleString& theFileName,
long theLineNumber,
const SimpleString& expected,
const SimpleString& actual)
: testName (theTestName),
fileName (theFileName),
lineNumber (theLineNumber)
{
const char *part1 = "expected ";
const char *part3 = " but was: ";
char *stage = new char [strlen (part1)
+ expected.size ()
+ strlen (part3)
+ actual.size ()
+ 1];
sprintf(stage, "%s%s%s%s",
part1,
expected.asCharString(),
part3,
actual.asCharString());
message = SimpleString(stage);
delete stage;
}

59
CppUnitLite/Failure.h
View File

@ -21,37 +21,50 @@
///////////////////////////////////////////////////////////////////////////////
#ifndef FAILURE_H
#define FAILURE_H
#include "SimpleString.h"
#pragma once
#include <string>
class Failure
{
public:
Failure (const SimpleString& theTestName,
const SimpleString& theFileName,
long theLineNumber,
const SimpleString& theCondition);
Failure (const std::string& theTestName,
const std::string& theFileName,
long theLineNumber,
const std::string& theCondition)
: message (theCondition),
testName (theTestName),
fileName (theFileName),
lineNumber (theLineNumber)
{
}
Failure (const SimpleString& theTestName,
const SimpleString& theFileName,
long theLineNumber,
const SimpleString& expected,
const SimpleString& actual);
Failure (const SimpleString& theTestName,
const SimpleString& theFileName,
const SimpleString& theCondition);
Failure (const std::string& theTestName,
const std::string& theFileName,
const std::string& theCondition)
: message (theCondition),
testName (theTestName),
fileName (theFileName),
lineNumber (-1)
{
}
SimpleString message;
SimpleString testName;
SimpleString fileName;
Failure (const std::string& theTestName,
const std::string& theFileName,
long theLineNumber,
const std::string& expected,
const std::string& actual)
: message("expected " + expected + " but was: " + actual),
testName (theTestName),
fileName (theFileName),
lineNumber (theLineNumber)
{
}
std::string message;
std::string testName;
std::string fileName;
long lineNumber;
};
#endif

115
CppUnitLite/SimpleString.cpp
View File

@ -1,115 +0,0 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#include "SimpleString.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
static const int DEFAULT_SIZE = 20;
SimpleString::SimpleString ()
: buffer_(new char [1])
{
buffer_ [0] = '\0';
}
SimpleString::SimpleString (const char *otherBuffer)
: buffer_ (new char [strlen (otherBuffer) + 1])
{
strcpy (buffer_, otherBuffer);
}
SimpleString::SimpleString (const SimpleString& other)
{
buffer_ = new char [other.size() + 1];
strcpy(buffer_, other.buffer_);
}
SimpleString SimpleString::operator= (const SimpleString& other)
{
delete buffer_;
buffer_ = new char [other.size() + 1];
strcpy(buffer_, other.buffer_);
return *this;
}
SimpleString SimpleString::operator+ (const SimpleString& other)
{
SimpleString ret;
delete ret.buffer_;
ret.buffer_ = new char [size() + other.size() - 1];
strcpy(ret.buffer_, buffer_);
strcat(ret.buffer_, other.buffer_);
return ret;
}
char *SimpleString::asCharString () const
{
return buffer_;
}
int SimpleString::size() const
{
return strlen (buffer_);
}
SimpleString::~SimpleString ()
{
delete [] buffer_;
}
bool operator== (const SimpleString& left, const SimpleString& right)
{
return !strcmp (left.asCharString (), right.asCharString ());
}
SimpleString StringFrom (bool value)
{
char buffer [sizeof ("false") + 1];
sprintf (buffer, "%s", value ? "true" : "false");
return SimpleString(buffer);
}
SimpleString StringFrom (const char *value)
{
return SimpleString(value);
}
SimpleString StringFrom (long value)
{
char buffer [DEFAULT_SIZE];
sprintf (buffer, "%ld", value);
return SimpleString(buffer);
}
SimpleString StringFrom (double value)
{
char buffer [DEFAULT_SIZE];
sprintf (buffer, "%lg", value);
return SimpleString(buffer);
}
SimpleString StringFrom (const SimpleString& value)
{
return SimpleString(value);
}

57
CppUnitLite/SimpleString.h
View File

@ -1,57 +0,0 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
///////////////////////////////////////////////////////////////////////////////
//
// SIMPLESTRING.H
//
// One of the design goals of CppUnitLite is to compilation with very old C++
// compilers. For that reason, I've added a simple string class that provides
// only the operations needed in CppUnitLite.
//
///////////////////////////////////////////////////////////////////////////////
#ifndef SIMPLE_STRING
#define SIMPLE_STRING
class SimpleString
{
friend bool operator== (const SimpleString& left, const SimpleString& right);
public:
SimpleString ();
SimpleString (const char *value);
SimpleString (const SimpleString& other);
~SimpleString ();
SimpleString operator= (const SimpleString& other);
SimpleString operator+ (const SimpleString& other);
char *asCharString () const;
int size() const;
private:
char *buffer_;
};
SimpleString StringFrom (bool value);
SimpleString StringFrom (const char *value);
SimpleString StringFrom (long value);
SimpleString StringFrom (double value);
SimpleString StringFrom (const SimpleString& other);
#endif

22
CppUnitLite/Test.cpp
View File

@ -10,21 +10,21 @@
* -------------------------------------------------------------------------- */
#include "Test.h"
#include "TestRegistry.h"
#include "TestResult.h"
#include "Failure.h"
#include <boost/lexical_cast.hpp>
Test::Test (const SimpleString& testName)
: name_ (testName)
Test::Test (const std::string& testName)
: name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true)
{
TestRegistry::addTest (this);
}
Test::Test (const SimpleString& testName, const SimpleString& filename, long lineNumber, bool safeCheck = true)
: name_(testName), filename_(filename), lineNumber_(lineNumber), safeCheck_(safeCheck)
Test::Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck)
: name_(testName), next_(0), filename_(filename), lineNumber_(lineNumber), safeCheck_(safeCheck)
{
TestRegistry::addTest (this);
}
@ -40,31 +40,31 @@ void Test::setNext(Test *test)
next_ = test;
}
bool Test::check(long expected, long actual, TestResult& result, const SimpleString& fileName, long lineNumber)
bool Test::check(long expected, long actual, TestResult& result, const std::string& fileName, long lineNumber)
{
if (expected == actual)
return true;
result.addFailure (
Failure (
name_,
StringFrom (__FILE__),
boost::lexical_cast<std::string> (__FILE__),
__LINE__,
StringFrom (expected),
StringFrom (actual)));
boost::lexical_cast<std::string> (expected),
boost::lexical_cast<std::string> (actual)));
return false;
}
bool Test::check(const SimpleString& expected, const SimpleString& actual, TestResult& result, const SimpleString& fileName, long lineNumber)
bool Test::check(const std::string& expected, const std::string& actual, TestResult& result, const std::string& fileName, long lineNumber)
{
if (expected == actual)
return true;
result.addFailure (
Failure (
name_,
StringFrom (__FILE__),
boost::lexical_cast<std::string> (__FILE__),
__LINE__,
expected,
actual));

42
CppUnitLite/Test.h
View File

@ -23,17 +23,15 @@
#include <cmath>
#include "SimpleString.h"
#include <boost/lexical_cast.hpp>
class TestResult;
class Test
{
public:
Test (const SimpleString& testName);
Test (const SimpleString& testName, const SimpleString& filename, long lineNumber, bool safeCheck);
Test (const std::string& testName);
Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck);
virtual ~Test() {};
virtual void run (TestResult& result) = 0;
@ -41,19 +39,19 @@ public:
void setNext(Test *test);
Test *getNext () const;
SimpleString getName() const {return name_;}
SimpleString getFilename() const {return filename_;}
std::string getName() const {return name_;}
std::string getFilename() const {return filename_;}
long getLineNumber() const {return lineNumber_;}
bool safe() const {return safeCheck_;}
protected:
bool check (long expected, long actual, TestResult& result, const SimpleString& fileName, long lineNumber);
bool check (const SimpleString& expected, const SimpleString& actual, TestResult& result, const SimpleString& fileName, long lineNumber);
bool check (long expected, long actual, TestResult& result, const std::string& fileName, long lineNumber);
bool check (const std::string& expected, const std::string& actual, TestResult& result, const std::string& fileName, long lineNumber);
SimpleString name_;
std::string name_;
Test *next_;
SimpleString filename_;
std::string filename_;
long lineNumber_; /// This is the line line number of the test, rather than the a single check
bool safeCheck_;
@ -102,17 +100,17 @@ protected:
#define THROWS_EXCEPTION(condition)\
{ try { condition; \
result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Didn't throw: ") + StringFrom(#condition))); \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \
return; } \
catch (...) {} }
#define CHECK_EXCEPTION(condition, exception_name)\
{ try { condition; \
result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Didn't throw: ") + StringFrom(#condition))); \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \
return; } \
catch (exception_name& e) {} \
catch (exception_name&) {} \
catch (...) { \
result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Wrong exception: ") + StringFrom(#condition) + StringFrom(", expected: ") + StringFrom(#exception_name))); \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast<std::string>(#condition) + boost::lexical_cast<std::string>(", expected: ") + boost::lexical_cast<std::string>(#exception_name))); \
return; } }
#define EQUALITY(expected,actual)\
@ -120,21 +118,21 @@ protected:
result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
#define CHECK_EQUAL(expected,actual)\
{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, StringFrom(expected), StringFrom(actual))); }
{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expected), boost::lexical_cast<std::string>(actual))); }
#define LONGS_EQUAL(expected,actual)\
{ long actualTemp = actual; \
long expectedTemp = expected; \
if ((expectedTemp) != (actualTemp)) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, StringFrom(expectedTemp), \
StringFrom(actualTemp))); return; } }
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expectedTemp), \
boost::lexical_cast<std::string>(actualTemp))); return; } }
#define DOUBLES_EQUAL(expected,actual,threshold)\
{ double actualTemp = actual; \
double expectedTemp = expected; \
if (fabs ((expectedTemp)-(actualTemp)) > threshold) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, \
StringFrom((double)expectedTemp), StringFrom((double)actualTemp))); return; } }
boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std::string>((double)actualTemp))); return; } }
/* EXPECTs: tests will continue running after a failure */
@ -146,15 +144,15 @@ StringFrom((double)expectedTemp), StringFrom((double)actualTemp))); return; } }
{ long actualTemp = actual; \
long expectedTemp = expected; \
if ((expectedTemp) != (actualTemp)) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, StringFrom(expectedTemp), \
StringFrom(actualTemp))); } }
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expectedTemp), \
boost::lexical_cast<std::string>(actualTemp))); } }
#define EXPECT_DOUBLES_EQUAL(expected,actual,threshold)\
{ double actualTemp = actual; \
double expectedTemp = expected; \
if (fabs ((expectedTemp)-(actualTemp)) > threshold) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, \
StringFrom((double)expectedTemp), StringFrom((double)actualTemp))); } }
boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std::string>((double)actualTemp))); } }
#define FAIL(text) \

5
CppUnitLite/TestRegistry.cpp
View File

@ -17,7 +17,6 @@
#include "Failure.h"
#include "TestResult.h"
#include "TestRegistry.h"
#include "SimpleString.h"
void TestRegistry::addTest (Test *test)
{
@ -65,12 +64,12 @@ int TestRegistry::run (TestResult& result)
// catch standard exceptions and derivatives
result.addFailure(
Failure(test->getName(), test->getFilename(), test->getLineNumber(),
SimpleString("Exception: ") + SimpleString(e.what())));
std::string("Exception: ") + std::string(e.what())));
} catch (...) {
// catch all other exceptions
result.addFailure(
Failure(test->getName(), test->getFilename(), test->getLineNumber(),
SimpleString("ExceptionThrown!")));
"ExceptionThrown!"));
}
}
else {

8
CppUnitLite/TestResult.cpp
View File

@ -32,16 +32,16 @@ void TestResult::addFailure (const Failure& failure)
if (failure.lineNumber < 0) // allow for no line number
fprintf (stdout, "%s%s%s%s\n",
"Failure: \"",
failure.message.asCharString (),
failure.message.c_str (),
"\" in ",
failure.fileName.asCharString ());
failure.fileName.c_str ());
else
fprintf (stdout, "%s%s%ld%s%s%s\n",
failure.fileName.asCharString(), // Format matches Eclipse error flagging
failure.fileName.c_str(), // Format matches Eclipse error flagging
":",
failure.lineNumber,
": Failure: \"",
failure.message.asCharString(),
failure.message.c_str(),
"\" ");
failureCount++;

17
README
View File

@ -37,7 +37,7 @@ of licensing in LICENSE and as follows:
- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library
- http://www.cise.ufl.edu/research/sparse
- Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
- Eigen 3.0.5: General C++ matrix and linear algebra library
- Eigen 3.1: General C++ matrix and linear algebra library
- Licenced under LGPL v3, provided in gtsam/3rdparty/Eigen/COPYING.LGPL
@ -51,17 +51,20 @@ Important Installation Notes
1)
GTSAM requires the following libraries to be installed on your system:
- BOOST version 1.40 or greater (install through Linux repositories or MacPorts)
- BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
- Cmake version 2.6 or higher
- Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
Tested compilers
- GCC 4.2-4.7
- Clang 2.9-3.2
- OSX GCC 4.2
- MSVC 2010, 2012
Tested systems:
- Ubuntu 11.04, 11.10, 12.04
- MacOS 10.6, 10.7
- Windows 7
2)
GTSAM makes extensive use of debug assertions, and we highly recommend you work
@ -71,8 +74,8 @@ will run up to 10x faster in Release mode! See the end of this document for
additional debugging tips.
3)
GTSAM has Doxygen documentation. To generate, run the the script
makedoc.sh.
GTSAM has Doxygen documentation. To generate, run 'make doc' from your
build directory.
4)
The instructions below install the library to the default system install path and
@ -147,6 +150,12 @@ just the geometry tests, run "make check.geometry". Individual tests can be run
appending ".run" to the name of the test, for example, to run testMatrix, run
"make testMatrix.run".
MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your
shell's PATH environment variable. mex is installed with matlab at
$MATLABROOT/bin/mex
$MATLABROOT can be found by executing the command 'matlabroot' in MATLAB
Debugging tips:
Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks

79
doc/CMakeLists.txt Normal file
View File

@ -0,0 +1,79 @@
# Doxygen documentation configuration
option(GTSAM_BUILD_DOCS "Enable/Disable building of doxygen docs" ON)
# configure doxygen
option(GTSAM_BUILD_DOC_HTML "Enable/Disable doxygen HTML output" ON)
option(GTSAM_BUILD_DOC_LATEX "Enable/Disable doxygen LaTeX output" OFF)
# add a target to generate API documentation with Doxygen
if (GTSAM_BUILD_DOCS)
# Convert configuration to YES/NO variables
if (GTSAM_BUILD_DOC_HTML)
set(GTSAM_BUILD_DOC_HTML_YN "YES")
else()
set(GTSAM_BUILD_DOC_HTML_YN "NO")
endif()
if (GTSAM_BUILD_DOC_LATEX)
set(GTSAM_BUILD_DOC_LATEX_YN "YES")
else()
set(GTSAM_BUILD_DOC_LATEX_YN "NO")
endif()
# GTSAM core subfolders
set(gtsam_doc_subdirs
gtsam/base
gtsam/geometry
gtsam/inference
gtsam/discrete
gtsam/linear
gtsam/nonlinear
gtsam/slam
gtsam
)
# Optional GTSAM_UNSTABLE subfolders
set(gtsam_unstable_doc_subdirs
gtsam_unstable/base
gtsam_unstable/discrete
gtsam_unstable/linear
gtsam_unstable/nonlinear
gtsam_unstable/slam
gtsam_unstable/dynamics
gtsam_unstable
)
# Build a list of folders to include depending on build options
set(doc_subdirs ${gtsam_doc_subdirs})
if (GTSAM_BUILD_UNSTABLE)
list(APPEND doc_subdirs ${gtsam_unstable_doc_subdirs})
endif()
if (GTSAM_BUILD_EXAMPLES)
list(APPEND doc_subdirs examples)
endif()
# From subfolders, build a list with whitespace separation of paths
set(GTSAM_DOXYGEN_INPUT_PATHS "")
foreach(dir ${doc_subdirs})
set(GTSAM_DOXYGEN_INPUT_PATHS "${GTSAM_DOXYGEN_INPUT_PATHS} ${PROJECT_SOURCE_DIR}/${dir}")
endforeach()
# Generate Doxyfile
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY)
# Add target to actually build documentation as configured
add_custom_target(doc
${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
COMMENT "Generating API documentation with Doxygen" VERBATIM
)
# Clean target
add_custom_target(doc_clean
COMMAND
cmake -E remove_directory ${CMAKE_CURRENT_SOURCE_DIR}/latex
COMMAND
cmake -E remove_directory ${CMAKE_CURRENT_SOURCE_DIR}/html
COMMENT "Removing Doxygen documentation"
)
endif()

24
Doxyfile → doc/Doxyfile.in
View File

@ -32,7 +32,7 @@ PROJECT_NAME = gtsam
# This could be handy for archiving the generated documentation or
# if some version control system is used.
PROJECT_NUMBER = 2.0.0
PROJECT_NUMBER = @GTSAM_VERSION_MAJOR@.@GTSAM_VERSION_MINOR@.@GTSAM_VERSION_PATCH@
# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer
@ -52,7 +52,7 @@ PROJECT_LOGO =
# If a relative path is entered, it will be relative to the location
# where doxygen was started. If left blank the current directory will be used.
OUTPUT_DIRECTORY = doc
OUTPUT_DIRECTORY = .
# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create
# 4096 sub-directories (in 2 levels) under the output directory of each output
@ -597,7 +597,7 @@ WARNINGS = YES
# for undocumented members. If EXTRACT_ALL is set to YES then this flag will
# automatically be disabled.
WARN_IF_UNDOCUMENTED = YES
WARN_IF_UNDOCUMENTED = NO
# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for
# potential errors in the documentation, such as not documenting some
@ -638,19 +638,7 @@ WARN_LOGFILE =
# directories like "/usr/src/myproject". Separate the files or directories
# with spaces.
INPUT = gtsam/base \
gtsam/geometry \
gtsam/inference \
gtsam/discrete \
gtsam/linear \
gtsam/nonlinear \
gtsam
# gtsam/slam \
# gtsam_unstable/slam \
# gtsam_unstable/base \
# gtsam_unstable/geometry \
# gtsam_unstable/dynamics \
# gtsam_unstable
INPUT = @GTSAM_DOXYGEN_INPUT_PATHS@
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
@ -880,7 +868,7 @@ IGNORE_PREFIX =
# If the GENERATE_HTML tag is set to YES (the default) Doxygen will
# generate HTML output.
GENERATE_HTML = YES
GENERATE_HTML = @GTSAM_BUILD_DOC_HTML_YN@
# The HTML_OUTPUT tag is used to specify where the HTML docs will be put.
# If a relative path is entered the value of OUTPUT_DIRECTORY will be
@ -1234,7 +1222,7 @@ SERVER_BASED_SEARCH = NO
# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will
# generate Latex output.
GENERATE_LATEX = NO
GENERATE_LATEX = @GTSAM_BUILD_DOC_LATEX_YN@
# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put.
# If a relative path is entered the value of OUTPUT_DIRECTORY will be

0
DoxygenLayout.xml → doc/DoxygenLayout.xml
View File

4065
doc/LieGroups.lyx Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/LieGroups.pdf Normal file
View File

Binary file not shown.

170
doc/cholesky.lyx Normal file
View File

@ -0,0 +1,170 @@
#LyX 1.6.7 created this file. For more info see http://www.lyx.org/
\lyxformat 345
\begin_document
\begin_header
\textclass article
\use_default_options true
\language english
\inputencoding auto
\font_roman default
\font_sans default
\font_typewriter default
\font_default_family default
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\paperfontsize default
\use_hyperref false
\papersize default
\use_geometry false
\use_amsmath 1
\use_esint 1
\cite_engine basic
\use_bibtopic false
\paperorientation portrait
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\defskip medskip
\quotes_language english
\papercolumns 1
\papersides 1
\paperpagestyle default
\tracking_changes false
\output_changes false
\author ""
\author ""
\end_header
\begin_body
\begin_layout Section
Basic solving with Cholesky
\end_layout
\begin_layout Standard
Solving a linear least-squares system:
\begin_inset Formula \[
\arg\min_{x}\left\Vert Ax-b\right\Vert ^{2}\]
\end_inset
Set derivative equal to zero:
\begin_inset Formula \begin{align*}
0 & =2A^{T}\left(Ax-b\right)\\
0 & =A^{T}Ax-A^{T}b\end{align*}
\end_inset
For comparison, with QR we do
\begin_inset Formula \begin{align*}
0 & =R^{T}Q^{T}QRx-R^{T}Qb\\
& =R^{T}Rx-R^{T}Qb\\
Rx & =Qb\\
x & =R^{-1}Qb\end{align*}
\end_inset
But with Cholesky we do
\begin_inset Formula \begin{align*}
0 & =R^{T}RR^{T}Rx-R^{T}Rb\\
& =R^{T}Rx-b\\
& =Rx-R^{-T}b\\
x & =R^{-1}R^{-T}b\end{align*}
\end_inset
\end_layout
\begin_layout Section
Frontal (rank-deficient) solving with Cholesky
\end_layout
\begin_layout Standard
To do multi-frontal elimination, we decompose into rank-deficient conditionals.
\begin_inset Formula \[
\left[\begin{array}{cccccc}
\cdot & \cdot & \cdot & \cdot & \cdot & \cdot\\
\cdot & \cdot & \cdot & \cdot & \cdot & \cdot\\
\cdot & \cdot & \cdot & \cdot & \cdot & \cdot\end{array}\right]\to\]
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula \[
\left[\begin{array}{cc}
R^{T} & 0\\
S^{T} & C^{T}\end{array}\right]\left[\begin{array}{cc}
R & S\\
0 & C\end{array}\right]=\left[\begin{array}{cc}
F^{T}F & F^{T}G\\
G^{T}F & G^{T}G\end{array}\right]\]
\end_inset
\end_layout
\begin_layout Standard
\begin_inset space ~
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula \[
R^{T}R=F^{T}F\]
\end_inset
\end_layout
\begin_layout Standard
\begin_inset space ~
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula \begin{align*}
R^{T}S & =F^{T}G\\
S & =R^{-T}F^{T}G\end{align*}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset space ~
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula \begin{align*}
S^{T}S+C^{T}C & =G^{T}G\\
G^{T}FR^{-1}R^{-T}F^{T}G+C^{T}C & =G^{T}G\\
G^{T}QRR^{-1}R^{-T}R^{T}Q^{T}G+C^{T}C & =G^{T}G\\
\textbf{if }R\textbf{ is invertible, }G^{T}G+C^{T}C & =G^{T}G\\
C^{T}C & =0\end{align*}
\end_inset
\end_layout
\end_body
\end_document

BIN
doc/images/circular.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/circular.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.5 KiB

1302
doc/images/gtsam-structure.graffle Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/images/gtsam-structure.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/n-steps.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/n-steps.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.4 KiB

294
doc/macros.lyx Normal file
View File

@ -0,0 +1,294 @@
#LyX 1.6.5 created this file. For more info see http://www.lyx.org/
\lyxformat 345
\begin_document
\begin_header
\textclass article
\use_default_options true
\language english
\inputencoding auto
\font_roman default
\font_sans default
\font_typewriter default
\font_default_family default
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\paperfontsize default
\use_hyperref false
\papersize default
\use_geometry false
\use_amsmath 1
\use_esint 1
\cite_engine basic
\use_bibtopic false
\paperorientation portrait
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\defskip medskip
\quotes_language english
\papercolumns 1
\papersides 1
\paperpagestyle default
\tracking_changes false
\output_changes false
\author ""
\author ""
\end_header
\begin_body
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
Derivatives
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}}
{\frac{\partial#1}{\partial#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
{#1\biggr\rvert_{#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
{\at{\deriv{#1}{#2}}{#3}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
Lie Groups
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\xhat}{\hat{x}}
{\hat{x}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\yhat}{\hat{y}}
{\hat{y}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Ad}[1]{Ad_{#1}}
{Ad_{#1}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\define}{\stackrel{\Delta}{=}}
{\stackrel{\Delta}{=}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\gg}{\mathfrak{g}}
{\mathfrak{g}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Rn}{\mathbb{R}^{n}}
{\mathbb{R}^{n}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SO(2)
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
{\mathfrak{\mathbb{R}^{2}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SOtwo}{SO(2)}
{SO(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sotwo}{\mathfrak{so(2)}}
{\mathfrak{so(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\that}{\hat{\theta}}
{\hat{\theta}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\skew}[1]{[#1]_{+}}
{[#1]_{+}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SE(2)
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\SEtwo}{SE(2)}
{SE(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\setwo}{\mathfrak{se(2)}}
{\mathfrak{se(2)}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SO(3)
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}}
{\mathfrak{\mathbb{R}^{3}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SOthree}{SO(3)}
{SO(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sothree}{\mathfrak{so(3)}}
{\mathfrak{so(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\what}{\hat{\omega}}
{\hat{\omega}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SE(3)
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}}
{\mathfrak{\mathbb{R}^{6}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SEthree}{SE(3)}
{SE(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sethree}{\mathfrak{se(3)}}
{\mathfrak{se(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\xihat}{\hat{\xi}}
{\hat{\xi}}
\end_inset
\end_layout
\end_body
\end_document

5746
doc/math.lyx Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/math.pdf Normal file
View File

Binary file not shown.

25
examples/CMakeLists.txt
View File

@ -1,4 +1,6 @@
add_custom_target(examples)
if(NOT MSVC)
add_custom_target(examples)
endif()
# Build example executables
FILE(GLOB example_srcs "*.cpp")
@ -6,10 +8,25 @@ foreach(example_src ${example_srcs} )
get_filename_component(example_base ${example_src} NAME_WE)
set( example_bin ${example_base} )
message(STATUS "Adding Example ${example_bin}")
add_dependencies(examples ${example_bin})
if(NOT MSVC)
add_dependencies(examples ${example_bin})
endif()
add_executable(${example_bin} ${example_src})
# Disable building during make all/install
if (GTSAM_DISABLE_EXAMPLES_ON_INSTALL)
set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
target_link_libraries(${example_bin} gtsam-static)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
if(NOT MSVC)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
endif()
# Set up Visual Studio folder
if(MSVC)
set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples")
endif()
endforeach(example_src)
add_subdirectory(vSLAMexample)

110
examples/CameraResectioning.cpp
View File

@ -17,86 +17,82 @@
*/
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace gtsam::noiseModel;
using symbol_shorthand::X;
/**
* Unary factor for the pose.
* Unary factor on the unknown pose, resulting from meauring the projection of
* a known 3D point in the image
*/
class ResectioningFactor: public NoiseModelFactor1<Pose3> {
typedef NoiseModelFactor1<Pose3> Base;
typedef NoiseModelFactor1<Pose3> Base;
shared_ptrK K_; // camera's intrinsic parameters
Point3 P_; // 3D point on the calibration rig
Point2 p_; // 2D measurement of the 3D point
shared_ptrK K_; // camera's intrinsic parameters
Point3 P_; // 3D point on the calibration rig
Point2 p_; // 2D measurement of the 3D point
public:
ResectioningFactor(const SharedNoiseModel& model, const Symbol& key,
const shared_ptrK& calib, const Point2& p, const Point3& P) :
Base(model, key), K_(calib), P_(P), p_(p) {
}
virtual ~ResectioningFactor() {}
/// Construct factor given known point P and its projection p
ResectioningFactor(const SharedNoiseModel& model, const Key& key,
const shared_ptrK& calib, const Point2& p, const Point3& P) :
Base(model, key), K_(calib), P_(P), p_(p) {
}
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H = boost::none) const {
SimpleCamera camera(*K_, X);
Point2 reprojectionError(camera.project(P_, H) - p_);
return reprojectionError.vector();
}
/// evaluate the error
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
Point2 reprojectionError(camera.project(P_, H) - p_);
return reprojectionError.vector();
}
};
/*******************************************************************************/
/**
* Camera: f = 1.0, Image: 100x100, center: 50.0, 50.0
/*******************************************************************************
* Camera: f = 1, Image: 100x100, center: 50, 50.0
* Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
* Known landmarks:
* 3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
* Perfect measurements:
* 2D Point: (55,45) (45,45) (45,55) (55,55)
*/
*******************************************************************************/
int main(int argc, char* argv[]) {
/* read camera intrinsic parameters */
shared_ptrK calib(new Cal3_S2(1.0, 1.0, 0, 50.0, 50.0));
/* read camera intrinsic parameters */
shared_ptrK calib(new Cal3_S2(1, 1, 0, 50, 50));
/* create keys for variables */
// we have only 1 variable to solve: the camera pose
Symbol X('x',1);
/* 1. create graph */
NonlinearFactorGraph graph;
/* 1. create graph */
NonlinearFactorGraph graph;
/* 2. add factors to the graph */
// add measurement factors
SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 45), Point3(10, 10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 45), Point3(-10, 10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 55), Point3(-10, -10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 55), Point3(10, -10, 0)));
/* 2. add factors to the graph */
// add measurement factors
SharedDiagonal measurementNoise = sharedSigmas(Vector_(2, 0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
factor = boost::shared_ptr<ResectioningFactor>(new ResectioningFactor(
measurementNoise, X, calib, Point2(55.0, 45.0), Point3(10.0, 10.0, 0.0)));
graph.push_back(factor);
factor = boost::shared_ptr<ResectioningFactor>(new ResectioningFactor(
measurementNoise, X, calib, Point2(45.0, 45.0), Point3(-10.0, 10.0, 0.0)));
graph.push_back(factor);
factor = boost::shared_ptr<ResectioningFactor>(new ResectioningFactor(
measurementNoise, X, calib, Point2(45.0, 55.0), Point3(-10.0, -10.0, 0.0)));
graph.push_back(factor);
factor = boost::shared_ptr<ResectioningFactor>(new ResectioningFactor(
measurementNoise, X, calib, Point2(55.0, 55.0), Point3(10.0, -10.0, 0.0)));
graph.push_back(factor);
/* 3. Create an initial estimate for the camera pose */
Values initial;
initial.insert(X(1),
Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));
/* 3. Create an initial estimate for the camera pose */
Values initial;
initial.insert(X, Pose3(Rot3(1.,0.,0.,
0.,-1.,0.,
0.,0.,-1.), Point3(0.,0.,2.0)));
/* 4. Optimize the graph using Levenberg-Marquardt*/
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("Final result:\n");
/* 4. Optimize the graph using Levenberg-Marquardt*/
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("Final result: ");
return 0;
return 0;
}

BIN
examples/Data/Balbianello/BalbianelloMedium-1.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 100 KiB

BIN
examples/Data/Balbianello/BalbianelloMedium-1.key.gz Normal file
View File

Binary file not shown.

BIN
examples/Data/Balbianello/BalbianelloMedium-2.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 91 KiB

BIN
examples/Data/Balbianello/BalbianelloMedium-2.key.gz Normal file
View File

Binary file not shown.

BIN
examples/Data/Balbianello/BalbianelloMedium-3.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 100 KiB

BIN
examples/Data/Balbianello/BalbianelloMedium-3.key.gz Normal file
View File

Binary file not shown.

BIN
examples/Data/Balbianello/BalbianelloMedium-4.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 94 KiB

BIN
examples/Data/Balbianello/BalbianelloMedium-4.key.gz Normal file
View File

Binary file not shown.

BIN
examples/Data/Balbianello/BalbianelloMedium-5.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 84 KiB

BIN
examples/Data/Balbianello/BalbianelloMedium-5.key.gz Normal file
View File

Binary file not shown.

1
examples/Data/VO_calibration.txt Normal file
View File

@ -0,0 +1 @@
721.5377 721.5377 0.0 609.5593 172.854 0.537150588

26
examples/Data/VO_camera_poses_large.txt Normal file
View File

@ -0,0 +1,26 @@
1 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
2 0.999997 -0.00240146 0.00061075 0.00314304 0.0024019 0.999997 -0.00071977 0.00414596 -0.00060902 0.000721235 1 0.95998 0 0 0 1
3 0.999993 -0.00371801 0.000595621 0.00280572 0.00371612 0.999988 0.00314729 0.00981461 -0.000607315 -0.00314506 0.999995 1.91967 0 0 0 1
4 0.999981 -0.00619252 -0.000161076 0.00419847 0.00619374 0.999939 0.00918605 0.0120306 0.000104181 -0.00918687 0.999958 2.87292 0 0 0 1
5 0.999948 -0.0100179 -0.00177824 0.00741887 0.010034 0.999907 0.00925915 0.0175144 0.00168532 -0.00927651 0.999956 3.8158 0 0 0 1
6 0.999913 -0.0128105 -0.00298253 0.00649213 0.0128298 0.999896 0.00655406 0.0221085 0.00289826 -0.00659176 0.999974 4.75923 0 0 0 1
7 0.999901 -0.0135442 -0.00393541 0.00202365 0.0135567 0.999903 0.00315259 0.0274932 0.00389233 -0.00320563 0.999987 5.69454 0 0 0 1
8 0.999901 -0.0131232 -0.00504356 -0.00337588 0.0131365 0.99991 0.00261558 0.0312754 0.00500879 -0.00268157 0.999984 6.62905 0 0 0 1
9 0.999898 -0.0127525 -0.00649016 -0.00834529 0.0127815 0.999908 0.00443934 0.0363095 0.00643296 -0.00452184 0.999969 7.56509 0 0 0 1
10 0.999891 -0.0126057 -0.0076649 -0.0140367 0.012663 0.999892 0.00747804 0.0406566 0.00756981 -0.00757428 0.999943 8.50119 0 0 0 1
11 0.99988 -0.012916 -0.00853732 -0.0314704 0.0129905 0.999878 0.00872879 0.0502508 0.00842353 -0.00883865 0.999925 9.44031 0 0 0 1
12 0.999851 -0.0144077 -0.00947329 -0.0383227 0.0144958 0.999852 0.00930193 0.0627772 0.00933787 -0.00943787 0.999912 10.3736 0 0 0 1
13 0.999838 -0.0146038 -0.0105024 -0.0473225 0.0147016 0.999849 0.00930217 0.0696137 0.010365 -0.00945507 0.999902 11.2945 0 0 0 1
14 0.999827 -0.0148804 -0.0111878 -0.0611688 0.0149688 0.999857 0.00785918 0.0760175 0.0110692 -0.00802528 0.999907 12.2087 0 0 0 1
15 0.999804 -0.015194 -0.0126986 -0.0750261 0.0152691 0.999866 0.0058357 0.0827557 0.0126082 -0.00602846 0.999902 13.1154 0 0 0 1
16 0.999759 -0.0166787 -0.0142956 -0.0893963 0.0167446 0.99985 0.00450347 0.0886196 0.0142183 -0.00474176 0.999888 14.0218 0 0 0 1
17 0.999738 -0.0164046 -0.0159548 -0.110797 0.0164665 0.999857 0.00375294 0.0939735 0.0158909 -0.00401468 0.999866 14.9288 0 0 0 1
18 0.999731 -0.0152362 -0.0175058 -0.134602 0.0152899 0.999879 0.00293826 0.0949048 0.0174589 -0.00320513 0.999842 15.8347 0 0 0 1
19 0.999721 -0.013717 -0.0192196 -0.158411 0.0137759 0.999901 0.00293494 0.0977547 0.0191774 -0.00319889 0.999811 16.7344 0 0 0 1
20 0.999683 -0.0142193 -0.0207555 -0.182009 0.0142987 0.999891 0.00368405 0.101695 0.0207008 -0.00397966 0.999778 17.6352 0 0 0 1
21 0.999659 -0.0137778 -0.0221853 -0.207494 0.01389 0.999891 0.00491306 0.105966 0.0221152 -0.00521954 0.999742 18.5239 0 0 0 1
22 0.999637 -0.013439 -0.0233607 -0.237562 0.0135682 0.999893 0.0053802 0.112022 0.023286 -0.00569521 0.999713 19.408 0 0 0 1
23 0.999585 -0.0146657 -0.0247774 -0.266515 0.0147976 0.999877 0.00515018 0.116865 0.0246988 -0.00551469 0.99968 20.2912 0 0 0 1
24 0.999541 -0.0147341 -0.0264658 -0.293414 0.0148605 0.999879 0.00458559 0.12245 0.0263951 -0.00497678 0.999639 21.1689 0 0 0 1
25 0.999508 -0.0146117 -0.0277547 -0.320586 0.0147561 0.999879 0.00500356 0.126076 0.0276782 -0.00541065 0.999602 22.0409 0 0 0 1
26 0.999474 -0.0145772 -0.0289811 -0.347714 0.0147851 0.999866 0.00697181 0.131533 0.0288756 -0.00739663 0.999556 22.9037 0 0 0 1

8189
examples/Data/VO_stereo_factors_large.txt Normal file
View File

File diff suppressed because it is too large Load Diff

1
examples/Data/calib.txt
View File

@ -1 +0,0 @@
800 600 1119.61507797 1119.61507797 399.5 299.5

611
examples/Data/example.graph Normal file
View File

@ -0,0 +1,611 @@
VERTEX2 0 -0.13824042322 0.342115061624 -0.21557075232
VERTEX2 1 0.612650361503 0.369015313751 -0.0563461272597
VERTEX2 2 1.45479903751 0.32013063469 0.00237213120454
VERTEX2 3 2.15898096669 0.470294509791 0.2252884021
VERTEX2 4 2.88516818219 0.975843235486 0.663228096686
VERTEX2 5 3.54878752013 1.42348114163 0.514657357344
VERTEX2 6 4.32268502365 1.77291535029 0.359562734992
VERTEX2 7 5.08813168428 2.06549657048 0.478125988701
VERTEX2 8 5.82496111116 2.35969982503 0.422228187736
VERTEX2 9 6.60694009323 2.69135662887 0.387034318241
VERTEX2 10 7.23925137037 3.104611283 0.512008508154
VERTEX2 11 7.88977565307 3.52369876229 0.510071235528
VERTEX2 12 8.46649417534 4.12269969397 0.748925805491
VERTEX2 13 9.02278033295 4.72424791132 0.856233672115
VERTEX2 14 9.51752434143 5.39695844275 0.935937098771
VERTEX2 15 10.0446733918 5.98414466425 0.827804203493
VERTEX2 16 10.7598348236 6.44960011929 0.66804473602
VERTEX2 17 11.5972376522 6.80537766408 0.434941288594
VERTEX2 18 12.2250075039 7.20442152894 0.60418193616
VERTEX2 19 12.8678138507 7.70255061643 0.721822794329
VERTEX2 20 13.5570195199 8.15843137362 0.593196573979
VERTEX2 21 14.105649437 8.68489938394 0.640245465981
VERTEX2 22 14.8224385492 9.03493116691 0.389518546051
VERTEX2 23 15.5843623557 9.40302979333 0.425220242351
VERTEX2 24 16.2288322902 9.91680608172 0.666877924504
VERTEX2 25 16.7734636078 10.4312050251 0.681531027672
VERTEX2 26 17.4387968671 11.005129183 0.768715640688
VERTEX2 27 17.7890621789 11.6956218678 1.10988383528
VERTEX2 28 18.1363236735 12.3803438454 1.15258075093
VERTEX2 29 18.5588734995 13.1408718358 1.04155126412
VERTEX2 30 18.9922514403 13.8160705369 0.971438456359
VERTEX2 31 19.4325629424 14.4733850988 0.934520345955
VERTEX2 32 19.9599388848 15.0464180472 0.877139653946
VERTEX2 33 20.5469053533 15.5710340353 0.832297735476
VERTEX2 34 21.2487400511 16.0027088196 0.400422858988
VERTEX2 35 22.0354445454 16.279918955 0.319643226898
VERTEX2 36 22.791287229 16.5499150181 0.433961446308
VERTEX2 37 23.5369676303 16.8796783232 0.412377133313
VERTEX2 38 24.3284326258 17.0889935946 0.247486633995
VERTEX2 39 25.022695509 17.4036693591 0.418515661559
VERTEX2 40 25.833691896 17.5333774597 0.242076471575
VERTEX2 41 26.509920511 17.8164161072 0.330656267986
VERTEX2 42 27.2102127101 18.1210604608 0.510746644634
VERTEX2 43 27.9408557181 18.4814891494 0.453985699035
VERTEX2 44 28.6169766716 18.8182098691 0.458777942952
VERTEX2 45 29.4313889637 19.1357097768 0.315177512361
VERTEX2 46 30.0878533291 19.5935012258 0.599592761269
VERTEX2 47 30.6308494244 20.2191621501 0.791897780929
VERTEX2 48 31.2734501775 20.7120615592 0.660037247468
VERTEX2 49 31.9206533665 21.0772297685 0.5072035147
VERTEX2 50 32.6763238236 21.4307681859 0.454485923064
VERTEX2 51 33.3148481476 21.8561680514 0.60984431497
VERTEX2 52 34.1013046603 22.2191493052 0.390376111344
VERTEX2 53 34.8953983726 22.3514742253 0.21440693077
VERTEX2 54 35.5398222802 22.7545971294 0.615814590831
VERTEX2 55 36.0796597873 23.3482683055 0.770181973449
VERTEX2 56 36.7331436274 23.8425596089 0.686311813947
VERTEX2 57 37.3595434784 24.3051907322 0.642633326683
VERTEX2 58 37.9821262299 24.7824436801 0.670645829824
VERTEX2 59 38.5783823068 25.3597188555 0.700563120761
VERTEX2 60 39.1778265015 25.9111323369 0.739828088912
VERTEX2 61 39.7774314984 26.468354909 0.760112883167
VERTEX2 62 40.5142302821 26.7603415627 0.360997494201
VERTEX2 63 41.3046572615 26.9763392108 0.294108606273
VERTEX2 64 42.0961911474 26.9894909267 0.0261721941131
VERTEX2 65 42.8993485692 27.0228858541 -0.0101699718322
VERTEX2 66 43.7802018787 26.9183386714 -0.14998582283
VERTEX2 67 44.4852457598 26.6914007663 -0.229610091357
VERTEX2 68 45.2817862376 26.3996944287 -0.403989633144
VERTEX2 69 45.9356734923 26.0051290697 -0.524619871446
VERTEX2 70 46.6446168067 25.600148756 -0.514124183817
VERTEX2 71 47.3746747907 25.2783070597 -0.393217999666
VERTEX2 72 48.0468975618 24.858529404 -0.611511747817
VERTEX2 73 48.5985868491 24.2739248983 -0.750544536918
VERTEX2 74 49.0527438314 23.5710791408 -0.961680491481
VERTEX2 75 49.2855884713 22.8045393649 -1.26366594654
VERTEX2 76 49.5531551426 22.07091907 -1.20968280224
VERTEX2 77 49.7661651916 21.2825192392 -1.31976079475
VERTEX2 78 50.0808782733 20.5442674319 -1.12044697254
VERTEX2 79 50.2411613948 19.8192859683 -1.32310987107
VERTEX2 80 50.593374374 19.0401532836 -1.19010897017
VERTEX2 81 50.7996996995 18.2237817778 -1.25581427155
VERTEX2 82 51.1212445439 17.5292614731 -1.12361470203
VERTEX2 83 51.4506022502 16.8176851053 -1.26886160199
VERTEX2 84 51.6718980908 16.0314246481 -1.20207903627
VERTEX2 85 51.7763413724 15.2475638806 -1.37580476659
VERTEX2 86 51.7436299376 14.470393643 -1.57551900737
VERTEX2 87 51.7823620936 13.662469339 -1.58332101723
VERTEX2 88 51.7876820152 12.8254876297 -1.54396226933
VERTEX2 89 51.8506699031 12.0601624148 -1.49704318637
VERTEX2 90 52.1025342746 11.3090918983 -1.27677489091
VERTEX2 91 52.3751696328 10.5860629809 -1.18997203951
VERTEX2 92 52.7492358918 9.85554108611 -1.06464614342
VERTEX2 93 53.0454987108 9.0309169672 -1.09735731166
VERTEX2 94 53.5294752309 8.30487726603 -0.996640361599
EDGE2 0 1 0.717448855412 0.179994080198 0.13818539933 1 0 1 1 0 0
EDGE2 1 2 0.837626285811 -0.00455175621601 0.0472991985156 1 0 1 1 0 0
EDGE2 2 3 0.730277063669 0.138122839345 0.195071775287 1 0 1 1 0 0
EDGE2 3 4 0.820088167497 0.330077088561 0.419082044433 1 0 1 1 0 0
EDGE2 4 5 0.751370583006 -0.0273254868576 -0.228659037893 1 0 1 1 0 0
EDGE2 5 6 0.840179304794 -0.0492096503989 -0.165641208116 1 0 1 1 0 0
EDGE2 6 7 0.834845292082 0.0123885559434 0.125976751144 1 0 1 1 0 0
EDGE2 7 8 0.787371504945 -0.0559773501633 -0.0755976318713 1 0 1 1 0 0
EDGE2 8 9 0.927708591397 -0.0476716890116 -0.00287962007188 1 0 1 1 0 0
EDGE2 9 10 0.730378261921 0.17317279657 0.138282893502 1 0 1 1 0 0
EDGE2 10 11 0.779303803898 0.0853449755561 0.0323688320359 1 0 1 1 0 0
EDGE2 11 12 0.78273752447 0.300201637472 0.246750324389 1 0 1 1 0 0
EDGE2 12 13 0.791566966819 0.0845024659404 0.0706094824868 1 0 1 1 0 0
EDGE2 13 14 0.839568888481 0.0868682543031 0.0707684617219 1 0 1 1 0 0
EDGE2 14 15 0.773151053081 -0.0593185180517 -0.155366269405 1 0 1 1 0 0
EDGE2 15 16 0.865064493339 -0.175806171924 -0.17949133873 1 0 1 1 0 0
EDGE2 16 17 0.911877312319 -0.185936696416 -0.268755813818 1 0 1 1 0 0
EDGE2 17 18 0.770300068296 0.126048087534 0.188926402795 1 0 1 1 0 0
EDGE2 18 19 0.836050157131 0.0586909108092 0.103845037354 1 0 1 1 0 0
EDGE2 19 20 0.802346917562 -0.147928636285 -0.093306893258 1 0 1 1 0 0
EDGE2 20 21 0.757849343112 0.0715772513539 0.124155654615 1 0 1 1 0 0
EDGE2 21 22 0.775750933534 -0.196756273535 -0.298936959752 1 0 1 1 0 0
EDGE2 22 23 0.871271438269 0.0573046191775 0.0353456305698 1 0 1 1 0 0
EDGE2 23 24 0.811030742162 0.182063773111 0.250823572639 1 0 1 1 0 0
EDGE2 24 25 0.746714037369 0.0325617212022 -0.0276665993655 1 0 1 1 0 0
EDGE2 25 26 0.86122739728 -0.00477108497939 0.0654936733039 1 0 1 1 0 0
EDGE2 26 27 0.743470468243 0.258017697 0.326840047412 1 0 1 1 0 0
EDGE2 27 28 0.767611305804 -0.0194473809016 0.015974781367 1 0 1 1 0 0
EDGE2 28 29 0.862923844436 -0.072552727637 -0.101230904939 1 0 1 1 0 0
EDGE2 29 30 0.798343874527 -0.0100123616381 -0.0690217895568 1 0 1 1 0 0
EDGE2 30 31 0.797880458664 0.010391602905 -0.0148972103321 1 0 1 1 0 0
EDGE2 31 32 0.7909503515 -0.129294597268 -0.0650424809131 1 0 1 1 0 0
EDGE2 32 33 0.842504611493 -0.175416599555 -0.0602555708183 1 0 1 1 0 0
EDGE2 33 34 0.850215628436 -0.25776734023 -0.430013888881 1 0 1 1 0 0
EDGE2 34 35 0.878503147997 -0.0904880193607 -0.110294282873 1 0 1 1 0 0
EDGE2 35 36 0.856359797661 -0.00248016167379 0.151938705998 1 0 1 1 0 0
EDGE2 36 37 0.832014541489 -0.0130179735184 0.0190247158611 1 0 1 1 0 0
EDGE2 37 38 0.79003682341 -0.11167056714 -0.209191335194 1 0 1 1 0 0
EDGE2 38 39 0.773095034914 0.115088515991 0.198916260227 1 0 1 1 0 0
EDGE2 39 40 0.868778586384 -0.268010831047 -0.216977740006 1 0 1 1 0 0
EDGE2 40 41 0.747080875885 0.111312520032 0.12949742181 1 0 1 1 0 0
EDGE2 41 42 0.778805446629 0.021318220948 0.192566907397 1 0 1 1 0 0
EDGE2 42 43 0.805554382015 0.0232777384249 -0.0694087339945 1 0 1 1 0 0
EDGE2 43 44 0.813443633444 0.0640947959252 0.0147897258654 1 0 1 1 0 0
EDGE2 44 45 0.880420396037 -0.0605322428822 -0.144031989178 1 0 1 1 0 0
EDGE2 45 46 0.827332486742 0.280288429131 0.328067436679 1 0 1 1 0 0
EDGE2 46 47 0.815610897482 0.224126154957 0.19211589295 1 0 1 1 0 0
EDGE2 47 48 0.829375613726 -0.13990131371 -0.15863308512 1 0 1 1 0 0
EDGE2 48 49 0.756790221176 -0.0537543626573 -0.14995378239 1 0 1 1 0 0
EDGE2 49 50 0.864033920652 -0.060820449151 -0.0470197844462 1 0 1 1 0 0
EDGE2 50 51 0.747576670836 0.115572803882 0.111904734712 1 0 1 1 0 0
EDGE2 51 52 0.801571192348 -0.200227517424 -0.271734247998 1 0 1 1 0 0
EDGE2 52 53 0.783342882846 -0.216604931502 -0.232376474041 1 0 1 1 0 0
EDGE2 53 54 0.716633103688 0.208120785595 0.398955080153 1 0 1 1 0 0
EDGE2 54 55 0.773727687761 0.144824498948 0.159143610152 1 0 1 1 0 0
EDGE2 55 56 0.779100726172 -0.140449927137 -0.0179275725938 1 0 1 1 0 0
EDGE2 56 57 0.77536053616 -0.0727835270276 -0.0370113971051 1 0 1 1 0 0
EDGE2 57 58 0.792809168417 -0.00330191812217 0.0632619937894 1 0 1 1 0 0
EDGE2 58 59 0.789336249422 0.0499678920653 0.04511737545 1 0 1 1 0 0
EDGE2 59 60 0.799312335356 0.0154078296169 0.0686385071901 1 0 1 1 0 0
EDGE2 60 61 0.841385096251 -0.0451402806057 -0.00137586137994 1 0 1 1 0 0
EDGE2 61 62 0.73727689023 -0.335159174575 -0.434654147977 1 0 1 1 0 0
EDGE2 62 63 0.849739889254 -0.101852726335 -0.0546134930344 1 0 1 1 0 0
EDGE2 63 64 0.732343387025 -0.152861749502 -0.277235132206 1 0 1 1 0 0
EDGE2 64 65 0.778127849356 0.0424893245594 -0.103498695967 1 0 1 1 0 0
EDGE2 65 66 0.847762865635 -0.152071812994 -0.150846679463 1 0 1 1 0 0
EDGE2 66 67 0.803448897426 -0.0786018488252 -0.100750096392 1 0 1 1 0 0
EDGE2 67 68 0.869574260331 -0.067393723035 -0.227113129725 1 0 1 1 0 0
EDGE2 68 69 0.700122812394 -0.127175450528 -0.145967973471 1 0 1 1 0 0
EDGE2 69 70 0.741569767586 0.0209821877481 0.0347431035841 1 0 1 1 0 0
EDGE2 70 71 0.782413802829 0.0140327998855 0.120620056225 1 0 1 1 0 0
EDGE2 71 72 0.726382907259 -0.174488223666 -0.216565330734 1 0 1 1 0 0
EDGE2 72 73 0.786270332597 -0.222893898151 -0.0947849029055 1 0 1 1 0 0
EDGE2 73 74 0.872600785936 -0.157300465738 -0.187369985951 1 0 1 1 0 0
EDGE2 74 75 0.745108268557 -0.24332540853 -0.256205853911 1 0 1 1 0 0
EDGE2 75 76 0.741656734669 0.0584297856637 0.0684660276348 1 0 1 1 0 0
EDGE2 76 77 0.83107553466 -0.068222280325 -0.101097480556 1 0 1 1 0 0
EDGE2 77 78 0.838396530029 0.105042823302 0.218131052621 1 0 1 1 0 0
EDGE2 78 79 0.76082044773 -0.162843454224 -0.195114595262 1 0 1 1 0 0
EDGE2 79 80 0.8724369566 0.157719970278 0.155580359369 1 0 1 1 0 0
EDGE2 80 81 0.89087135963 -0.0673911030734 -0.043743726621 1 0 1 1 0 0
EDGE2 81 82 0.771001809797 0.114512192599 0.150286338457 1 0 1 1 0 0
EDGE2 82 83 0.812780357241 0.00395510362595 -0.215353852667 1 0 1 1 0 0
EDGE2 83 84 0.812252387954 -0.0274735142136 0.0152810139298 1 0 1 1 0 0
EDGE2 84 85 0.728059940228 -0.211739661884 -0.148376056446 1 0 1 1 0 0
EDGE2 85 86 0.743100348032 -0.205518475255 -0.220672724526 1 0 1 1 0 0
EDGE2 86 87 0.800837885496 0.0270216423016 -0.0121669543509 1 0 1 1 0 0
EDGE2 87 88 0.84523698212 0.025403746229 0.0393784439646 1 0 1 1 0 0
EDGE2 88 89 0.776634574418 0.0577931763852 0.0735885911249 1 0 1 1 0 0
EDGE2 89 90 0.795529423553 0.237162491279 0.23536520899 1 0 1 1 0 0
EDGE2 90 91 0.805787421901 0.0521263906349 0.0421550276299 1 0 1 1 0 0
EDGE2 91 92 0.821073980711 0.0501285496699 0.101339417467 1 0 1 1 0 0
EDGE2 92 93 0.852950616989 -0.144711794646 -0.0130042276003 1 0 1 1 0 0
EDGE2 93 94 0.86189702426 0.0935919937033 0.0835356089409 1 0 1 1 0 0
BR 0 144 0.185182458717 9.13212526936 0.0349 0.1
BR 1 144 -0.0129226894449 8.31812366009 0.0349 0.1
BR 2 137 -0.00718256171569 9.4510095689 0.0349 0.1
BR 2 144 -0.0402886026913 7.40183795114 0.0349 0.1
BR 3 110 -0.0749913933297 9.38512144583 0.0349 0.1
BR 3 137 -0.214302812649 8.91847706309 0.0349 0.1
BR 3 144 -0.331046619669 6.88216766504 0.0349 0.1
BR 4 110 -0.528773048781 8.70707069137 0.0349 0.1
BR 4 137 -0.721109439101 8.1019911191 0.0349 0.1
BR 4 144 -0.823882810817 6.3429714463 0.0349 0.1
BR 5 110 -0.440317029226 7.90152610245 0.0349 0.1
BR 5 137 -0.695335598589 7.36947399552 0.0349 0.1
BR 5 144 -0.836258509529 5.68573435009 0.0349 0.1
BR 6 110 -0.300578669402 7.30263189677 0.0349 0.1
BR 6 137 -0.615732878739 6.58070581163 0.0349 0.1
BR 6 144 -0.748502380201 4.97181349627 0.0349 0.1
BR 7 110 -0.457809242486 6.4057096724 0.0349 0.1
BR 7 137 -0.799831219908 6.09478184977 0.0349 0.1
BR 7 144 -0.964230560912 4.56454858228 0.0349 0.1
BR 8 110 -0.467348208232 5.49325886324 0.0349 0.1
BR 8 137 -0.807694275092 5.51772540214 0.0349 0.1
BR 8 144 -1.13536530576 3.81666726517 0.0349 0.1
BR 9 110 -0.518981300619 5.25851673305 0.0349 0.1
BR 9 137 -0.854409841336 5.08562424167 0.0349 0.1
BR 9 144 -1.28402784294 3.64150184656 0.0349 0.1
BR 10 110 -0.790651006069 4.290875248 0.0349 0.1
BR 10 137 -1.18130413181 4.71464967115 0.0349 0.1
BR 10 144 -1.5646244434 3.61089756748 0.0349 0.1
BR 11 110 -0.880642022432 3.98236035001 0.0349 0.1
BR 11 137 -1.29691369659 4.50238497278 0.0349 0.1
BR 11 144 -1.82713910938 3.78383778763 0.0349 0.1
BR 12 110 -1.32187002351 3.62433679399 0.0349 0.1
BR 12 137 -1.7145607617 4.46822989525 0.0349 0.1
BR 12 144 -2.21545709717 4.24105066636 0.0349 0.1
BR 13 110 -1.71296518505 3.57549906761 0.0349 0.1
BR 13 137 -1.98866377754 4.79459486414 0.0349 0.1
BR 13 144 -2.46140779298 4.93843797616 0.0349 0.1
BR 14 110 -1.99056042177 3.82339476594 0.0349 0.1
BR 14 137 -2.22640434465 5.305250162 0.0349 0.1
BR 14 144 -2.55677091106 5.46170314439 0.0349 0.1
BR 15 110 -2.04880614871 4.10480923415 0.0349 0.1
BR 15 137 -2.23779804266 5.87635629681 0.0349 0.1
BR 15 144 -2.6151098223 6.3515317913 0.0349 0.1
BR 16 110 -2.06003060806 4.42557132946 0.0349 0.1
BR 16 117 -1.35214243919 10.0761482514 0.0349 0.1
BR 16 137 -2.21401914815 6.16051869558 0.0349 0.1
BR 16 144 -2.53705251945 6.83886701303 0.0349 0.1
BR 17 110 -2.05946000954 4.70286658969 0.0349 0.1
BR 17 117 -1.24383519856 9.61361367685 0.0349 0.1
BR 17 137 -2.10879624347 6.45383801752 0.0349 0.1
BR 17 144 -2.37194735825 7.43285240943 0.0349 0.1
BR 18 110 -2.24565367321 5.05455899365 0.0349 0.1
BR 18 117 -1.4566998731 9.45407941567 0.0349 0.1
BR 18 137 -2.36776538577 7.09802331697 0.0349 0.1
BR 18 144 -2.63414325206 8.05544152173 0.0349 0.1
BR 19 110 -2.53165374141 5.63229552339 0.0349 0.1
BR 19 117 -1.69653853719 9.45427424857 0.0349 0.1
BR 19 137 -2.50848481788 7.55177276066 0.0349 0.1
BR 19 144 -2.79987374438 8.77345763541 0.0349 0.1
BR 20 110 -2.47990354001 6.35010296373 0.0349 0.1
BR 20 117 -1.62650378923 9.39686061139 0.0349 0.1
BR 20 137 -2.54701526227 8.32866970212 0.0349 0.1
BR 20 144 -2.63947313054 9.56517009268 0.0349 0.1
BR 21 110 -2.55850203983 6.96898661248 0.0349 0.1
BR 21 117 -1.72847175528 9.84266033626 0.0349 0.1
BR 21 130 0.664621658612 9.5759865835 0.0349 0.1
BR 21 137 -2.57262324612 9.07025812037 0.0349 0.1
BR 22 110 -2.46140069173 7.76173665587 0.0349 0.1
BR 22 117 -1.50767608255 9.8291133283 0.0349 0.1
BR 22 130 0.823885118652 8.81501192048 0.0349 0.1
BR 22 137 -2.38946943302 9.59604468387 0.0349 0.1
BR 23 110 -2.49582690441 8.28781200224 0.0349 0.1
BR 23 117 -1.72519040254 9.80915204869 0.0349 0.1
BR 23 130 0.931240309024 8.39880397639 0.0349 0.1
BR 24 110 -2.75856723917 9.10160958405 0.0349 0.1
BR 24 130 0.78416776229 7.76276053788 0.0349 0.1
BR 25 110 -2.80692285652 9.76642515575 0.0349 0.1
BR 25 130 0.775233070078 7.11275145442 0.0349 0.1
BR 26 130 0.790834643547 6.33736770408 0.0349 0.1
BR 27 130 0.534243817656 5.91578829565 0.0349 0.1
BR 28 112 0.0414364252573 9.90330083232 0.0349 0.1
BR 28 130 0.508210275456 5.21523105568 0.0349 0.1
BR 28 146 -0.845262619975 9.96746671173 0.0349 0.1
BR 29 112 0.183098888443 8.97611974568 0.0349 0.1
BR 29 130 0.714643071872 4.52800551501 0.0349 0.1
BR 29 146 -0.759652310647 9.34738787399 0.0349 0.1
BR 30 112 0.220803493857 8.38735907801 0.0349 0.1
BR 30 130 0.986350911973 3.98277990705 0.0349 0.1
BR 30 146 -0.778277445645 8.57965070765 0.0349 0.1
BR 31 112 0.299031878039 7.66323555915 0.0349 0.1
BR 31 130 1.24857419325 3.67126201123 0.0349 0.1
BR 31 146 -0.801974563814 8.07274519519 0.0349 0.1
BR 32 112 0.423997675694 6.73235131609 0.0349 0.1
BR 32 130 1.46838088131 3.46892248338 0.0349 0.1
BR 32 146 -0.79908260713 7.4588636461 0.0349 0.1
BR 33 112 0.476958671405 6.18741823661 0.0349 0.1
BR 33 130 1.75701275846 3.59510992657 0.0349 0.1
BR 33 146 -0.831662121031 7.04303707355 0.0349 0.1
BR 34 112 1.11053185407 5.73111181601 0.0349 0.1
BR 34 130 2.32852970858 4.06426280442 0.0349 0.1
BR 34 146 -0.46502078765 6.39400757176 0.0349 0.1
BR 35 112 1.29539336024 5.39706444208 0.0349 0.1
BR 35 129 -0.286317245678 9.78983149537 0.0349 0.1
BR 35 130 2.56479344603 4.74998169538 0.0349 0.1
BR 35 146 -0.532333579717 5.60699182585 0.0349 0.1
BR 36 112 1.32829018039 5.23245230162 0.0349 0.1
BR 36 129 -0.460072777958 8.93159378784 0.0349 0.1
BR 36 130 -3.72946554068 5.24707372009 0.0349 0.1
BR 36 146 -0.651976626401 5.00329187599 0.0349 0.1
BR 37 112 1.47981669566 4.97207303698 0.0349 0.1
BR 37 129 -0.3297608179 8.28473488253 0.0349 0.1
BR 37 130 -3.64959871445 6.0920051131 0.0349 0.1
BR 37 146 -0.761130816352 4.33535233295 0.0349 0.1
BR 38 112 1.79010072399 5.11134982 0.0349 0.1
BR 38 129 -0.239172851184 7.30972395375 0.0349 0.1
BR 38 130 -3.51623581001 7.035350049 0.0349 0.1
BR 38 132 0.0344695438909 9.16065906572 0.0349 0.1
BR 38 146 -0.729682501329 3.69383136681 0.0349 0.1
BR 39 112 1.80724225657 5.0712041146 0.0349 0.1
BR 39 129 -0.466822552134 6.71909069076 0.0349 0.1
BR 39 130 -3.58201828036 7.74976854347 0.0349 0.1
BR 39 132 -0.145489919023 8.26408270817 0.0349 0.1
BR 39 136 -0.373174247759 10.0594690013 0.0349 0.1
BR 39 146 -1.03884588422 3.05919083504 0.0349 0.1
BR 40 112 2.09018812193 5.6349471547 0.0349 0.1
BR 40 129 -0.347661923995 6.08661605953 0.0349 0.1
BR 40 130 -3.40788452199 8.33216602696 0.0349 0.1
BR 40 132 0.0183763853089 7.65498891306 0.0349 0.1
BR 40 136 -0.205928280437 9.23372153865 0.0349 0.1
BR 40 146 -1.15729572346 2.758937753 0.0349 0.1
BR 41 112 2.11156675384 6.17957324232 0.0349 0.1
BR 41 129 -0.513471163104 5.47917944802 0.0349 0.1
BR 41 130 -3.45321447468 8.96911724514 0.0349 0.1
BR 41 132 -0.00417850025153 6.7854687031 0.0349 0.1
BR 41 136 -0.306724739524 8.47672248547 0.0349 0.1
BR 41 146 -1.48971635217 2.63763674624 0.0349 0.1
BR 42 112 2.03992029635 6.31576122552 0.0349 0.1
BR 42 129 -0.830261802361 4.91295992246 0.0349 0.1
BR 42 130 -3.55865937637 9.89982911214 0.0349 0.1
BR 42 132 -0.236186305191 6.2509710589 0.0349 0.1
BR 42 136 -0.594332746928 7.68658045685 0.0349 0.1
BR 42 146 -1.89947156813 2.90103368108 0.0349 0.1
BR 43 112 2.20405052469 6.82505384798 0.0349 0.1
BR 43 129 -0.86677535825 4.24973929099 0.0349 0.1
BR 43 132 -0.170572630907 5.21697313753 0.0349 0.1
BR 43 136 -0.59926860855 6.96617850118 0.0349 0.1
BR 43 146 -2.18328873694 3.15420328364 0.0349 0.1
BR 44 112 2.28288455017 7.21970860023 0.0349 0.1
BR 44 129 -1.0660796602 3.60369990665 0.0349 0.1
BR 44 132 -0.269529469754 4.78179932008 0.0349 0.1
BR 44 136 -0.552094085176 6.39602268905 0.0349 0.1
BR 44 146 -2.34040228059 3.39346775306 0.0349 0.1
BR 45 112 2.53707604246 8.03041958241 0.0349 0.1
BR 45 129 -1.17710675346 3.31741261854 0.0349 0.1
BR 45 132 -0.169178567894 3.72737707244 0.0349 0.1
BR 45 136 -0.58982083798 5.65573127607 0.0349 0.1
BR 45 146 -2.31254599631 4.073315509 0.0349 0.1
BR 46 112 2.30336072906 8.53878391819 0.0349 0.1
BR 46 129 -1.56469510909 3.27257795455 0.0349 0.1
BR 46 132 -0.596947941988 3.01003728934 0.0349 0.1
BR 46 136 -0.972903852928 5.11198354755 0.0349 0.1
BR 46 145 -0.379034045173 9.44743347682 0.0349 0.1
BR 46 146 -2.71640793524 4.69101880432 0.0349 0.1
BR 47 112 -4.09979829598 8.93000904485 0.0349 0.1
BR 47 129 -2.08829636192 3.6048789119 0.0349 0.1
BR 47 132 -0.992144225485 2.55259283291 0.0349 0.1
BR 47 136 -1.37087445362 4.9788870276 0.0349 0.1
BR 47 138 0.471079307943 9.48838419239 0.0349 0.1
BR 47 145 -0.585304896045 8.55966650601 0.0349 0.1
BR 47 146 -2.86951126121 5.46138428744 0.0349 0.1
BR 48 112 -3.87787490571 9.29627530206 0.0349 0.1
BR 48 129 -2.08519281032 3.93461289407 0.0349 0.1
BR 48 132 -1.1882653081 2.02386021289 0.0349 0.1
BR 48 136 -1.37524100074 4.78257071668 0.0349 0.1
BR 48 138 0.596766880921 8.85284436333 0.0349 0.1
BR 48 145 -0.641491507579 7.95203729796 0.0349 0.1
BR 48 146 -2.75763166267 6.362323849 0.0349 0.1
BR 49 211 0.739516646502 9.8712550054 0.0349 0.1
BR 49 129 -2.14750043042 4.41467137273 0.0349 0.1
BR 49 132 -1.32001186912 1.81101852014 0.0349 0.1
BR 49 136 -1.3132413823 4.29282959482 0.0349 0.1
BR 49 138 0.852323239846 8.31832913318 0.0349 0.1
BR 49 145 -0.425206869031 7.30184295026 0.0349 0.1
BR 49 146 -2.81219083584 7.1999192447 0.0349 0.1
BR 50 211 0.835145641247 9.39883397423 0.0349 0.1
BR 50 129 -2.29739481979 4.84591520301 0.0349 0.1
BR 50 132 -1.69740609702 1.88108819843 0.0349 0.1
BR 50 136 -1.41813064673 4.22649063304 0.0349 0.1
BR 50 138 1.00506413746 7.80362591487 0.0349 0.1
BR 50 145 -0.455209251406 6.50387636211 0.0349 0.1
BR 50 146 -2.73782469254 7.87670364149 0.0349 0.1
BR 51 211 0.702762892786 8.87516504348 0.0349 0.1
BR 51 129 -2.47535256165 5.23986867221 0.0349 0.1
BR 51 132 -2.30239380502 2.18778407787 0.0349 0.1
BR 51 133 -0.721507862233 9.98554796724 0.0349 0.1
BR 51 136 -1.81775933595 4.26750754492 0.0349 0.1
BR 51 138 0.972975632993 7.33609223589 0.0349 0.1
BR 51 145 -0.655168730304 5.97764397657 0.0349 0.1
BR 51 146 -2.9008185124 8.52711403391 0.0349 0.1
BR 52 211 1.02160299745 8.39990010785 0.0349 0.1
BR 52 129 -2.38078665638 5.89398424924 0.0349 0.1
BR 52 132 -2.32150260829 2.7642509002 0.0349 0.1
BR 52 133 -0.593338340282 9.09248006087 0.0349 0.1
BR 52 136 -1.70907208704 4.52511614196 0.0349 0.1
BR 52 138 1.31486182491 7.00604753829 0.0349 0.1
BR 52 145 -0.623641599141 5.23461904841 0.0349 0.1
BR 52 146 -2.72475360758 9.52193132649 0.0349 0.1
BR 53 211 1.31559744632 8.11139198603 0.0349 0.1
BR 53 214 0.237146993035 9.72602918909 0.0349 0.1
BR 53 129 -2.29755087534 6.31115337961 0.0349 0.1
BR 53 132 -2.35974179857 3.19960631018 0.0349 0.1
BR 53 133 -0.394598671292 8.38412703631 0.0349 0.1
BR 53 136 -1.80773002423 4.40929640292 0.0349 0.1
BR 53 138 1.54926467893 6.88525427648 0.0349 0.1
BR 53 145 -0.474177899207 4.53003713639 0.0349 0.1
BR 54 211 0.984732803445 7.42786510356 0.0349 0.1
BR 54 214 -0.11180491587 9.02509825398 0.0349 0.1
BR 54 129 -2.74232381524 6.87541946089 0.0349 0.1
BR 54 132 -2.90610674415 3.90683353197 0.0349 0.1
BR 54 133 -0.946269617578 7.87816492515 0.0349 0.1
BR 54 136 -2.26079857458 5.05238790003 0.0349 0.1
BR 54 138 1.23881048754 6.66133430521 0.0349 0.1
BR 54 145 -0.947227352968 3.83527710037 0.0349 0.1
BR 55 211 0.898485222553 7.20091899213 0.0349 0.1
BR 55 214 -0.331475822555 8.15612623956 0.0349 0.1
BR 55 129 -2.94211283394 7.71377937916 0.0349 0.1
BR 55 132 -2.96263902277 4.72504746653 0.0349 0.1
BR 55 133 -1.19619007418 7.57872766791 0.0349 0.1
BR 55 136 -2.50797826148 5.6165181032 0.0349 0.1
BR 55 138 1.15570916703 6.30471479324 0.0349 0.1
BR 55 145 -1.34211765445 3.61160861965 0.0349 0.1
BR 56 211 1.15825542961 6.7888627078 0.0349 0.1
BR 56 214 -0.219470357269 7.60748335035 0.0349 0.1
BR 56 129 -2.8680390372 8.7065246753 0.0349 0.1
BR 56 132 -2.98769597419 5.64491656469 0.0349 0.1
BR 56 133 -1.18165204481 7.28168467592 0.0349 0.1
BR 56 136 -2.50662065879 6.33607011113 0.0349 0.1
BR 56 138 1.38983018292 6.2829567225 0.0349 0.1
BR 56 145 -1.48728342633 3.55073682465 0.0349 0.1
BR 57 211 1.31121936211 6.4499882023 0.0349 0.1
BR 57 214 -0.23903049767 6.70522585497 0.0349 0.1
BR 57 129 -2.9344494194 9.42282481723 0.0349 0.1
BR 57 132 -2.92810125756 6.33396803893 0.0349 0.1
BR 57 133 -1.20821854839 7.04533813918 0.0349 0.1
BR 57 136 -2.58342247807 6.79285980736 0.0349 0.1
BR 57 138 1.60454813339 6.18478768582 0.0349 0.1
BR 57 141 0.855478263576 9.81268967389 0.0349 0.1
BR 57 145 -1.64743462419 3.55197042884 0.0349 0.1
BR 58 211 1.3697376327 6.28814896116 0.0349 0.1
BR 58 214 -0.240573643222 6.00770973914 0.0349 0.1
BR 58 132 -3.04782555269 7.20609442047 0.0349 0.1
BR 58 133 -1.38654094358 6.89172239496 0.0349 0.1
BR 58 136 -2.71931807774 7.43484636746 0.0349 0.1
BR 58 138 1.67863856935 6.1507384544 0.0349 0.1
BR 58 141 0.976143688736 9.46697440494 0.0349 0.1
BR 58 145 -1.83870327644 3.65503744995 0.0349 0.1
BR 59 211 1.48699429679 6.08115940992 0.0349 0.1
BR 59 214 -0.415570760211 5.13438758848 0.0349 0.1
BR 59 132 -3.02827764182 7.96286566655 0.0349 0.1
BR 59 133 -1.53145859942 6.77610181098 0.0349 0.1
BR 59 136 -2.74531072749 8.32929527587 0.0349 0.1
BR 59 138 1.72057247223 6.26065897831 0.0349 0.1
BR 59 141 1.03071332211 8.88769595496 0.0349 0.1
BR 59 145 -2.08753542039 4.10672697789 0.0349 0.1
BR 60 211 1.59739021193 6.15588728463 0.0349 0.1
BR 60 214 -0.511168179873 4.33010644987 0.0349 0.1
BR 60 132 -3.09344355595 8.61019942961 0.0349 0.1
BR 60 133 -1.7654808189 6.71410728498 0.0349 0.1
BR 60 136 -2.77389441808 9.0499972807 0.0349 0.1
BR 60 138 1.8820286755 6.37121288506 0.0349 0.1
BR 60 141 1.07152819217 8.35085378616 0.0349 0.1
BR 60 145 -2.26379347264 4.5992746157 0.0349 0.1
BR 61 211 1.65733486617 6.06862058524 0.0349 0.1
BR 61 214 -0.592403718182 3.79555195012 0.0349 0.1
BR 61 116 -1.11564212635 9.76016152567 0.0349 0.1
BR 61 132 -3.08611036748 9.40173514366 0.0349 0.1
BR 61 133 -1.83574223151 6.77780820625 0.0349 0.1
BR 61 136 -2.81345972719 9.87046005411 0.0349 0.1
BR 61 138 2.01635034865 6.58454580038 0.0349 0.1
BR 61 141 1.05738477029 7.90059102737 0.0349 0.1
BR 61 145 -2.46764482041 5.23886327349 0.0349 0.1
BR 62 211 2.19981530481 6.55940364295 0.0349 0.1
BR 62 214 -0.330629210924 2.89895419258 0.0349 0.1
BR 62 116 -0.712532191251 9.34490097865 0.0349 0.1
BR 62 133 -1.50791588512 7.01329261316 0.0349 0.1
BR 62 138 2.42398978932 7.32781104937 0.0349 0.1
BR 62 141 1.56493435286 7.93355916389 0.0349 0.1
BR 62 145 -2.2278542462 5.49873142521 0.0349 0.1
BR 63 211 2.32173832925 7.0194270853 0.0349 0.1
BR 63 214 -0.386580845638 2.43330741085 0.0349 0.1
BR 63 116 -0.729079668132 8.53447098505 0.0349 0.1
BR 63 133 -1.57833951194 6.88281488712 0.0349 0.1
BR 63 138 2.57564520733 7.78656203108 0.0349 0.1
BR 63 141 1.81747758419 8.07098244115 0.0349 0.1
BR 63 145 -2.23878875365 5.92539391826 0.0349 0.1
BR 64 211 2.64430253079 7.83864538862 0.0349 0.1
BR 64 214 -0.0994208312564 1.44204544545 0.0349 0.1
BR 64 116 -0.55500504894 7.72325595942 0.0349 0.1
BR 64 133 -1.4536518654 6.65517115162 0.0349 0.1
BR 64 138 2.8401061616 8.57033338472 0.0349 0.1
BR 64 141 2.1957214779 8.46409667869 0.0349 0.1
BR 64 145 -2.02665785317 6.30435908086 0.0349 0.1
BR 65 211 2.77723756274 8.49481575652 0.0349 0.1
BR 65 214 -0.222004642925 0.769067027532 0.0349 0.1
BR 65 116 -0.476406751958 6.944593788 0.0349 0.1
BR 65 133 -1.5055349911 6.5435294725 0.0349 0.1
BR 65 138 2.89207363594 9.52197266067 0.0349 0.1
BR 65 139 0.690529531955 9.88828227669 0.0349 0.1
BR 65 141 2.20677044598 8.87273441824 0.0349 0.1
BR 65 145 -2.16442898949 6.62783981315 0.0349 0.1
BR 66 211 2.92708006505 9.25699933973 0.0349 0.1
BR 66 214 -2.70729617971 0.089927286145 0.0349 0.1
BR 66 116 -0.437916662456 6.37909317223 0.0349 0.1
BR 66 133 -1.53242050643 6.44340702808 0.0349 0.1
BR 66 139 0.911600005332 9.29148527033 0.0349 0.1
BR 66 141 2.43759073241 9.53382144492 0.0349 0.1
BR 66 145 -2.09539296277 7.1168381771 0.0349 0.1
BR 67 214 -3.07507169223 0.880638010099 0.0349 0.1
BR 67 116 -0.364201036385 5.70130017619 0.0349 0.1
BR 67 133 -1.5796828657 6.40788347591 0.0349 0.1
BR 67 139 1.09811562801 8.94633439107 0.0349 0.1
BR 67 145 -2.09672822137 7.34322150374 0.0349 0.1
BR 68 214 3.25749632058 1.71825216935 0.0349 0.1
BR 68 116 -0.250579061157 4.98724463734 0.0349 0.1
BR 68 133 -1.49770874602 6.29144792573 0.0349 0.1
BR 68 139 1.35862501974 8.66611776892 0.0349 0.1
BR 68 145 -2.0380754896 7.71319884521 0.0349 0.1
BR 69 214 3.36924028033 2.37881667021 0.0349 0.1
BR 69 114 0.965203878623 9.96554579176 0.0349 0.1
BR 69 116 -0.177465967368 4.16576217975 0.0349 0.1
BR 69 133 -1.58615525394 6.17247552536 0.0349 0.1
BR 69 139 1.6044244742 8.66323549571 0.0349 0.1
BR 69 145 -2.05356692956 8.23087471484 0.0349 0.1
BR 70 214 3.26447815983 3.50520447752 0.0349 0.1
BR 70 114 1.04557241854 9.48035082217 0.0349 0.1
BR 70 116 -0.174044759473 3.30434496879 0.0349 0.1
BR 70 133 -1.71878170033 6.12453562279 0.0349 0.1
BR 70 139 1.6988840667 8.90097770891 0.0349 0.1
BR 70 145 -2.09118659226 8.57946955435 0.0349 0.1
BR 71 214 3.17116978801 4.13326733054 0.0349 0.1
BR 71 114 0.971598066455 8.94056084471 0.0349 0.1
BR 71 116 -0.442084127811 2.65553820347 0.0349 0.1
BR 71 131 1.51291549191 9.93983683031 0.0349 0.1
BR 71 133 -1.92850538371 6.61503232087 0.0349 0.1
BR 71 139 1.62178936432 8.9275013656 0.0349 0.1
BR 71 145 -2.26077765156 8.81810560523 0.0349 0.1
BR 72 214 3.3483118075 5.07146259456 0.0349 0.1
BR 72 114 1.25024033184 8.65447650965 0.0349 0.1
BR 72 116 -0.29484144987 1.7807736885 0.0349 0.1
BR 72 133 -1.85932117232 6.54581267011 0.0349 0.1
BR 72 139 1.90675845297 9.03288373875 0.0349 0.1
BR 72 145 -2.11133037157 9.64278665008 0.0349 0.1
BR 73 214 3.42031481088 5.68604714096 0.0349 0.1
BR 73 114 1.49448393979 8.76928636352 0.0349 0.1
BR 73 116 -0.311282829508 0.986563086856 0.0349 0.1
BR 73 133 -1.81885055957 6.86219220059 0.0349 0.1
BR 73 139 2.19840475652 9.39629377331 0.0349 0.1
BR 73 145 -2.08434312084 10.019522072 0.0349 0.1
BR 74 214 3.53214111622 6.47416976793 0.0349 0.1
BR 74 219 1.125245923 9.70485712932 0.0349 0.1
BR 74 114 1.78271170313 8.74159061945 0.0349 0.1
BR 74 116 -0.496879055986 0.212551914458 0.0349 0.1
BR 74 133 -1.69837838721 6.62504018219 0.0349 0.1
BR 75 214 3.80548992485 6.97643942878 0.0349 0.1
BR 75 219 1.44648023525 9.61002619118 0.0349 0.1
BR 75 114 2.17495579726 9.36616344671 0.0349 0.1
BR 75 116 3.15139437828 0.640676998818 0.0349 0.1
BR 75 133 -1.47800625615 6.55213863832 0.0349 0.1
BR 76 214 3.64481450915 7.68409243587 0.0349 0.1
BR 76 219 1.55416548399 9.58187707528 0.0349 0.1
BR 76 114 2.22336594529 9.69979828032 0.0349 0.1
BR 76 116 3.12835380769 1.52693080051 0.0349 0.1
BR 76 133 -1.74190482626 6.46408688569 0.0349 0.1
BR 77 214 3.78271717671 8.33092440218 0.0349 0.1
BR 77 219 1.67410135616 9.71669336385 0.0349 0.1
BR 77 116 3.18266207552 2.40071229786 0.0349 0.1
BR 77 133 -1.70540467651 6.66269006969 0.0349 0.1
BR 78 214 3.42981856843 8.9840154373 0.0349 0.1
BR 78 219 1.55521791221 9.65971453808 0.0349 0.1
BR 78 116 3.08079550878 3.13311875201 0.0349 0.1
BR 78 133 -1.9629815922 7.01715815731 0.0349 0.1
BR 79 214 3.64301348008 9.64604876625 0.0349 0.1
BR 79 219 1.84313214295 9.8736833446 0.0349 0.1
BR 79 116 3.23976285086 3.82731244384 0.0349 0.1
BR 79 133 -1.92693348218 7.07982856181 0.0349 0.1
BR 80 219 1.82267971334 10.0181195912 0.0349 0.1
BR 80 116 3.0932606444 4.56053898399 0.0349 0.1
BR 80 133 -2.16850666747 7.82134702265 0.0349 0.1
BR 81 116 3.10963701499 5.36017902545 0.0349 0.1
BR 81 133 4.14615729787 7.91335159223 0.0349 0.1
BR 82 116 3.02427035132 6.3165782418 0.0349 0.1
BR 82 118 0.386637063745 9.30518727853 0.0349 0.1
BR 82 133 3.96654040143 8.46139182575 0.0349 0.1
BR 83 116 3.17577034597 6.95212398542 0.0349 0.1
BR 83 118 0.54486099103 8.70615519965 0.0349 0.1
BR 83 133 3.97631817437 9.0707544279 0.0349 0.1
BR 84 116 3.08672783226 7.73113294416 0.0349 0.1
BR 84 118 0.50758642684 7.92961801618 0.0349 0.1
BR 84 133 3.83740508413 9.53430891219 0.0349 0.1
BR 85 116 3.2851042494 8.61085233676 0.0349 0.1
BR 85 118 0.800702749373 7.297783814 0.0349 0.1
BR 86 116 3.46202134229 9.40431553868 0.0349 0.1
BR 86 118 1.02219323662 6.98093561867 0.0349 0.1
BR 86 125 0.894103907508 9.78156008486 0.0349 0.1
BR 87 118 1.17775566595 6.57332518044 0.0349 0.1
BR 87 125 0.953764386986 9.11980772782 0.0349 0.1
BR 88 118 1.23977118944 6.39248070013 0.0349 0.1
BR 88 125 0.988718137509 8.638143389 0.0349 0.1
BR 89 118 1.32805006899 6.07271098982 0.0349 0.1
BR 89 125 1.03116673027 8.19879638653 0.0349 0.1
BR 90 118 1.27747987129 5.7243455139 0.0349 0.1
BR 90 125 0.89663965971 7.9292423614 0.0349 0.1
BR 91 118 1.23616540573 5.42052023942 0.0349 0.1
BR 91 125 0.901333443965 7.46153346919 0.0349 0.1
BR 92 118 1.27418747645 5.10370611594 0.0349 0.1
BR 92 125 0.814247104386 6.78352263469 0.0349 0.1
BR 93 118 1.52925418993 5.08780303591 0.0349 0.1
BR 93 122 0.0350305409562 9.72894928235 0.0349 0.1
BR 93 125 0.990434800962 6.40937952378 0.0349 0.1
BR 94 118 1.57085863582 5.16163930514 0.0349 0.1
BR 94 122 -0.0746183892492 8.89244084523 0.0349 0.1
BR 94 125 0.977283745835 5.68611237558 0.0349 0.1

9
examples/Data/landmarks.txt
View File

@ -1,9 +0,0 @@
7
0 0 0 0
1 10 0 0
2 0 10 0
3 10 10 0
4 0 0 10
5 10 0 10
6 0 10 10

12
examples/Data/measurements.txt
View File

@ -1,12 +0,0 @@
10
1 ttpy10.feat
2 ttpy20.feat
3 ttpy30.feat
4 ttpy40.feat
5 ttpy50.feat
6 ttpy60.feat
7 ttpy70.feat
8 ttpy80.feat
9 ttpy90.feat
10 ttpy100.feat

12
examples/Data/poses.txt
View File

@ -1,12 +0,0 @@
10
1 ttpy10.pose
2 ttpy20.pose
3 ttpy30.pose
4 ttpy40.pose
5 ttpy50.pose
6 ttpy60.pose
7 ttpy70.pose
8 ttpy80.pose
9 ttpy90.pose
10 ttpy100.pose

4949
examples/Data/sphere2500.txt Normal file
View File

File diff suppressed because it is too large Load Diff

4949
examples/Data/sphere2500_groundtruth.txt Normal file
View File

File diff suppressed because it is too large Load Diff

10847
examples/Data/sphere_smallnoise.graph Normal file
View File

File diff suppressed because it is too large Load Diff

8
examples/Data/ttpy10.feat
View File

@ -1,8 +0,0 @@
7
6 424 190
4 399.553 240.574
2 422.974 248.632
5 480.082 258.082
3 496.146 264.634
0 399.5 299.5
1 475.915 317.106

4
examples/Data/ttpy10.pose
View File

@ -1,4 +0,0 @@
0.93969 0.24185 -0.24185 34.202
0.34202 -0.66446 0.66446 -93.969
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy100.feat
View File

@ -1,8 +0,0 @@
7
5 321.545 223.591
4 399.553 240.574
1 325.395 282.512
6 372.434 296.453
0 399.5 299.5
3 296.479 336.708
2 373.796 355.347

4
examples/Data/ttpy100.pose
View File

@ -1,4 +0,0 @@
-0.93969 -0.24185 0.24185 -34.202
-0.34202 0.66446 -0.66446 93.969
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy20.feat
View File

@ -1,8 +0,0 @@
7
6 448.854 198.317
4 399.574 240.532
2 446.39 257.049
5 467.479 276.146
3 509.674 289.86
0 399.5 299.5
1 463.827 335.115

4
examples/Data/ttpy20.pose
View File

@ -1,4 +0,0 @@
0.76604 0.45452 -0.45452 64.279
0.64279 -0.54168 0.54168 -76.604
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy30.feat
View File

@ -1,8 +0,0 @@
7
6 468.239 211.826
4 399.319 240.617
2 464.725 270.725
5 445.7 290.2
0 399.5 299.5
3 510.239 317.674
1 443.471 349.078

4
examples/Data/ttpy30.pose
View File

@ -1,4 +0,0 @@
0.5 0.61237 -0.61237 86.603
0.86603 -0.35355 0.35355 -50
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy40.feat
View File

@ -1,8 +0,0 @@
7
6 480.111 229.111
4 399.556 240.556
2 476.163 287.93
5 417.685 298.056
0 399.605 299.535
3 497.1 344.3
1 416.846 357.038

4
examples/Data/ttpy40.pose
View File

@ -1,4 +0,0 @@
0.17365 0.69636 -0.69636 98.481
0.98481 -0.12279 0.12279 -17.365
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy50.feat
View File

@ -1,8 +0,0 @@
7
4 399.429 240.551
6 482.812 248.312
5 387.364 298.964
0 399.578 299.556
2 478.63 307.391
1 387.98 357.804
3 470.922 366.471

4
examples/Data/ttpy50.pose
View File

@ -1,4 +0,0 @@
-0.17365 0.69636 -0.69636 98.481
0.98481 0.12279 -0.12279 17.365
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy60.feat
View File

@ -1,8 +0,0 @@
7
4 399.478 240.565
6 474.941 267.451
5 358.473 292.364
0 399.5 299.5
2 471.043 326.447
1 360.68 351.22
3 434.473 380.709

4
examples/Data/ttpy60.pose
View File

@ -1,4 +0,0 @@
-0.5 0.61237 -0.61237 86.603
0.86603 0.35355 -0.35355 50
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy70.feat
View File

@ -1,8 +0,0 @@
7
4 399.553 240.617
5 335.294 279.275
6 457.717 283.698
0 399.5 299.5
1 338.531 338.265
2 454.54 342.64
3 393.218 384.691

4
examples/Data/ttpy70.pose
View File

@ -1,4 +0,0 @@
-0.76604 0.45452 -0.45452 64.279
0.64279 0.54168 -0.54168 76.604
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy80.feat
View File

@ -1,8 +0,0 @@
7
4 399.5 240.5
5 320.667 261.958
6 432.389 295.111
0 399.5 299.5
1 324.653 320.898
2 430.5 353.96
3 352.737 377.474

4
examples/Data/ttpy80.pose
View File

@ -1,4 +0,0 @@
-0.93969 0.24185 -0.24185 34.202
0.34202 0.66446 -0.66446 93.969
0 -0.70711 -0.70711 100
0 0 0 1

8
examples/Data/ttpy90.feat
View File

@ -1,8 +0,0 @@
7
4 399.5 240.5
5 316 242.5
0 397 299.5
6 402.765 299.588
1 320.5 301.5
2 402.5 358.5
3 319 360.5

4
examples/Data/ttpy90.pose
View File

@ -1,4 +0,0 @@
-1 -0 0 0
0 0.70711 -0.70711 100
0 -0.70711 -0.70711 100
0 0 0 1

440
examples/Data/w100-odom.graph Normal file
View File

@ -0,0 +1,440 @@
VERTEX2 0 0 0 0
VERTEX2 1 0.995595 0.0837204 0.0146728
VERTEX2 2 2.0463 0.0352563 -0.0332615
VERTEX2 3 3.01173 0.00117694 -0.0153904
VERTEX2 4 4.00973 -0.0790194 -0.02875
VERTEX2 5 5.0196 -0.0664912 -0.039715
VERTEX2 6 5.02744 0.934341 1.51768
VERTEX2 7 5.04775 1.89323 1.51033
VERTEX2 8 5.15306 2.87418 1.49866
VERTEX2 9 5.22304 3.87497 1.48364
VERTEX2 10 5.23596 4.87041 1.47951
VERTEX2 11 4.24581 4.94796 2.99944
VERTEX2 12 3.22758 5.09551 2.99492
VERTEX2 13 2.19142 5.16887 2.98707
VERTEX2 14 1.1311 5.24329 2.97244
VERTEX2 15 0.158988 5.30427 2.95438
VERTEX2 16 -0.12526 4.26995 -1.76551
VERTEX2 17 -0.25431 3.29165 -1.77099
VERTEX2 18 -0.413555 2.30595 -1.76019
VERTEX2 19 -0.536073 1.30685 -1.75991
VERTEX2 20 -0.808468 0.302589 -1.76466
VERTEX2 21 0.165652 0.0941948 -0.204681
VERTEX2 22 1.09464 -0.0109951 -0.196319
VERTEX2 23 2.15691 -0.187264 -0.20809
VERTEX2 24 3.19115 -0.448207 -0.192611
VERTEX2 25 4.13241 -0.636131 -0.162687
VERTEX2 26 4.34104 0.512161 1.44413
VERTEX2 27 4.44536 1.59548 1.42312
VERTEX2 28 4.57241 2.60423 1.37618
VERTEX2 29 4.86591 3.53433 1.35616
VERTEX2 30 5.11392 4.4801 1.35985
VERTEX2 31 4.21358 4.6527 2.91174
VERTEX2 32 3.22654 4.82431 2.94929
VERTEX2 33 2.23774 5.00761 2.97485
VERTEX2 34 1.22736 5.20153 2.98412
VERTEX2 35 0.123255 5.26802 2.9767
VERTEX2 36 0.00632935 4.33864 -1.76346
VERTEX2 37 -0.10879 3.36173 -1.7901
VERTEX2 38 -0.321951 2.4384 -1.78648
VERTEX2 39 -0.543773 1.34377 -1.77905
VERTEX2 40 -0.658597 0.382801 -1.78632
VERTEX2 41 0.317835 0.0716759 -0.235552
VERTEX2 42 1.3002 -0.181376 -0.217406
VERTEX2 43 2.34947 -0.372484 -0.22532
VERTEX2 44 3.34502 -0.63322 -0.236028
VERTEX2 45 4.3567 -0.866613 -0.24513
VERTEX2 46 4.61743 0.074884 1.32649
VERTEX2 47 4.7283 1.10386 1.31325
VERTEX2 48 4.88942 2.06218 1.34353
VERTEX2 49 5.1224 3.05224 1.34
VERTEX2 50 5.37031 3.89715 1.33962
VERTEX2 51 4.40295 4.12271 2.91276
VERTEX2 52 3.40284 4.39024 2.9116
VERTEX2 53 2.43673 4.61578 2.93887
VERTEX2 54 1.51408 4.75597 2.91757
VERTEX2 55 0.505735 4.96923 2.92324
VERTEX2 56 0.256695 4.01175 -1.78543
VERTEX2 57 0.0116126 2.99092 -1.79932
VERTEX2 58 -0.153788 2.07052 -1.80614
VERTEX2 59 -0.31455 1.12993 -1.83961
VERTEX2 60 -0.593175 0.109086 -1.83163
VERTEX2 61 -1.51899 0.241119 2.87633
VERTEX2 62 -2.44041 0.564876 2.86065
VERTEX2 63 -3.38567 0.881461 2.88557
VERTEX2 64 -4.33636 1.15673 2.87003
VERTEX2 65 -5.3523 1.33309 2.88766
VERTEX2 66 -5.08987 2.28075 1.31311
VERTEX2 67 -4.8429 3.23082 1.32403
VERTEX2 68 -4.59105 4.20251 1.31499
VERTEX2 69 -4.38824 5.10708 1.32873
VERTEX2 70 -4.13462 5.98725 1.31402
VERTEX2 71 -5.15517 6.29412 2.90024
VERTEX2 72 -6.16852 6.57768 2.91315
VERTEX2 73 -7.19212 6.7759 2.90733
VERTEX2 74 -8.19083 6.96321 2.87753
VERTEX2 75 -9.21776 7.31653 2.85231
VERTEX2 76 -9.46536 6.44658 -1.86253
VERTEX2 77 -9.70116 5.60413 -1.86719
VERTEX2 78 -10.0031 4.63377 -1.8642
VERTEX2 79 -10.2892 3.70939 -1.85209
VERTEX2 80 -10.5468 2.73555 -1.86536
VERTEX2 81 -9.53131 2.42844 -0.348286
VERTEX2 82 -8.67728 2.03155 -0.326912
VERTEX2 83 -7.76543 1.7035 -0.340149
VERTEX2 84 -6.72871 1.23422 -0.332391
VERTEX2 85 -5.7871 0.960473 -0.298466
VERTEX2 86 -6.15287 -0.0181319 -1.84594
VERTEX2 87 -6.48069 -0.957942 -1.82387
VERTEX2 88 -6.74441 -1.90271 -1.83019
VERTEX2 89 -6.98263 -2.8633 -1.82672
VERTEX2 90 -7.25581 -3.79723 -1.7991
VERTEX2 91 -6.24938 -3.97274 -0.228747
VERTEX2 92 -5.2193 -4.22592 -0.231118
VERTEX2 93 -4.2791 -4.41808 -0.244412
VERTEX2 94 -3.27605 -4.63989 -0.262869
VERTEX2 95 -2.21195 -4.90921 -0.241867
VERTEX2 96 -1.9446 -3.94139 1.35224
VERTEX2 97 -1.7284 -2.88069 1.34979
VERTEX2 98 -1.4787 -1.9068 1.36613
VERTEX2 99 -1.32193 -0.941704 1.35348
EDGE2 1 0 -0.99879 0.0417574 -0.00818381 1 0 1 1 0 0
EDGE2 2 1 -1.00336 0.0235924 -0.0056968 1 0 1 1 0 0
EDGE2 3 2 -0.972181 0.0502932 -0.0342992 1 0 1 1 0 0
EDGE2 4 3 -1.03801 0.00907053 -0.0116629 1 0 1 1 0 0
EDGE2 5 4 -0.993225 0.0522372 0.00915452 1 0 1 1 0 0
EDGE2 6 5 -1.1127 0.00877534 -1.55251 1 0 1 1 0 0
EDGE2 7 6 -1.02681 0.0465162 -0.0118557 1 0 1 1 0 0
EDGE2 8 7 -0.917358 0.0291951 -0.0343225 1 0 1 1 0 0
EDGE2 9 8 -1.05727 -0.00948761 -0.0459367 1 0 1 1 0 0
EDGE2 10 9 -0.989284 0.0306925 -0.000322391 1 0 1 1 0 0
EDGE2 11 10 -0.997544 0.0340262 -1.59025 1 0 1 1 0 0
EDGE2 12 11 -0.900141 -0.0311005 0.0198686 1 0 1 1 0 0
EDGE2 13 12 -0.934983 -0.0793681 -0.00127435 1 0 1 1 0 0
EDGE2 14 13 -0.885324 -0.0619103 0.0131666 1 0 1 1 0 0
EDGE2 15 14 -0.90106 -0.0376226 -0.0082473 1 0 1 1 0 0
EDGE2 16 15 -0.992517 0.00499859 -1.61534 1 0 1 1 0 0
EDGE2 17 16 -0.975795 0.00721342 -0.00783327 1 0 1 1 0 0
EDGE2 18 17 -0.997573 0.0333984 0.0275033 1 0 1 1 0 0
EDGE2 19 18 -1.01687 0.04722 -0.0197328 1 0 1 1 0 0
EDGE2 19 0 0.973663 0.0205486 1.57776 1 0 1 1 0 0
EDGE2 20 19 -1.02103 0.00401273 0.0118729 1 0 1 1 0 0
EDGE2 20 1 0.050153 0.95087 1.5776 1 0 1 1 0 0
EDGE2 20 0 0.0746671 0.0345259 1.54892 1 0 1 1 0 0
EDGE2 21 2 1.04442 -0.0648639 0.0369546 1 0 1 1 0 0
EDGE2 21 1 -0.060912 -0.0429049 0.0281134 1 0 1 1 0 0
EDGE2 21 0 -1.03635 -0.0239619 0.0119243 1 0 1 1 0 0
EDGE2 21 20 -0.993726 -0.0523398 -1.58635 1 0 1 1 0 0
EDGE2 22 2 0.0369633 -0.0141374 0.0250929 1 0 1 1 0 0
EDGE2 22 3 1.06863 0.0015984 0.00161736 1 0 1 1 0 0
EDGE2 22 1 -0.998706 0.0305766 0.0318395 1 0 1 1 0 0
EDGE2 22 21 -1.03575 -0.0195948 0.0195394 1 0 1 1 0 0
EDGE2 23 4 1.02992 0.127035 -0.0346718 1 0 1 1 0 0
EDGE2 23 2 -0.969735 -0.0376916 -0.00768099 1 0 1 1 0 0
EDGE2 23 3 -0.0191715 -0.0462526 0.0191143 1 0 1 1 0 0
EDGE2 23 22 -0.923346 0.0731681 -0.0296362 1 0 1 1 0 0
EDGE2 24 5 0.947991 -0.0175548 -0.0253511 1 0 1 1 0 0
EDGE2 24 4 -0.0109939 0.0915607 -0.0086247 1 0 1 1 0 0
EDGE2 24 3 -1.14502 0.0593321 -0.00746596 1 0 1 1 0 0
EDGE2 24 23 -1.01657 -0.115682 0.0089996 1 0 1 1 0 0
EDGE2 25 6 -0.0597013 1.02123 1.59642 1 0 1 1 0 0
EDGE2 25 24 -1.09573 0.0614943 0.0261947 1 0 1 1 0 0
EDGE2 25 5 -0.0734935 -0.0017356 -0.00885563 1 0 1 1 0 0
EDGE2 25 4 -1.00036 -0.0171662 0.0202356 1 0 1 1 0 0
EDGE2 26 7 1.00531 0.00107148 0.0302732 1 0 1 1 0 0
EDGE2 26 25 -0.979494 0.0230425 -1.59852 1 0 1 1 0 0
EDGE2 26 6 -0.00315216 -0.0685196 0.0109497 1 0 1 1 0 0
EDGE2 26 5 -0.972334 0.0191047 -1.57567 1 0 1 1 0 0
EDGE2 27 8 0.898284 -0.0536875 0.0186598 1 0 1 1 0 0
EDGE2 27 7 0.0724439 -0.00529359 -0.000353825 1 0 1 1 0 0
EDGE2 27 6 -1.05569 0.0767555 -0.00615115 1 0 1 1 0 0
EDGE2 27 26 -0.967053 0.019671 0.00377432 1 0 1 1 0 0
EDGE2 28 9 1.01666 -0.0683645 -0.023268 1 0 1 1 0 0
EDGE2 28 27 -0.934108 -0.101065 0.00574705 1 0 1 1 0 0
EDGE2 28 8 0.00153443 -0.0194617 0.00533619 1 0 1 1 0 0
EDGE2 28 7 -1.0055 -0.0583432 -0.0150068 1 0 1 1 0 0
EDGE2 29 10 1.08105 0.00811641 -0.0127997 1 0 1 1 0 0
EDGE2 29 9 0.000458911 -0.119207 0.0094008 1 0 1 1 0 0
EDGE2 29 8 -1.03153 0.0730486 -0.00615654 1 0 1 1 0 0
EDGE2 29 28 -0.967347 0.0195529 -0.0185565 1 0 1 1 0 0
EDGE2 30 10 -0.0111261 -0.0223523 -0.0275291 1 0 1 1 0 0
EDGE2 30 11 0.0777909 0.988987 1.57983 1 0 1 1 0 0
EDGE2 30 29 -1.04043 0.0504065 0.0370536 1 0 1 1 0 0
EDGE2 30 9 -0.984437 -0.00337167 0.0067686 1 0 1 1 0 0
EDGE2 31 10 -0.988697 0.0379451 -1.55557 1 0 1 1 0 0
EDGE2 31 30 -1.00276 -0.102334 -1.55246 1 0 1 1 0 0
EDGE2 31 11 -0.0363097 0.0438826 0.00956678 1 0 1 1 0 0
EDGE2 31 12 1.07396 -0.0654312 -0.012633 1 0 1 1 0 0
EDGE2 32 31 -1.07853 -0.0168996 0.00213085 1 0 1 1 0 0
EDGE2 32 11 -1.01502 -0.0432938 -0.0288545 1 0 1 1 0 0
EDGE2 32 12 -0.00161924 -0.0369144 0.0119887 1 0 1 1 0 0
EDGE2 32 13 1.04694 0.02334 -0.0125276 1 0 1 1 0 0
EDGE2 33 32 -1.04882 -0.0552537 0.046005 1 0 1 1 0 0
EDGE2 33 12 -1.04043 -0.0558011 -0.0162327 1 0 1 1 0 0
EDGE2 33 13 0.0562588 0.104327 0.00415458 1 0 1 1 0 0
EDGE2 33 14 0.977863 0.0186884 0.0127758 1 0 1 1 0 0
EDGE2 34 33 -0.994151 0.0271125 -0.0029927 1 0 1 1 0 0
EDGE2 34 13 -0.992533 0.0446083 0.0139962 1 0 1 1 0 0
EDGE2 34 14 0.0338907 0.00424032 0.0260941 1 0 1 1 0 0
EDGE2 34 15 0.967129 -0.0598619 -0.0205361 1 0 1 1 0 0
EDGE2 35 14 -0.992147 -0.0193694 -0.0208901 1 0 1 1 0 0
EDGE2 35 34 -0.977902 0.0673003 0.0245254 1 0 1 1 0 0
EDGE2 35 15 0.0688948 0.0729152 -0.0423015 1 0 1 1 0 0
EDGE2 35 16 -0.0108759 0.943493 1.58518 1 0 1 1 0 0
EDGE2 36 35 -0.974289 -0.034236 -1.59368 1 0 1 1 0 0
EDGE2 36 15 -1.00822 0.00249408 -1.57297 1 0 1 1 0 0
EDGE2 36 16 -0.00217215 0.103762 -0.003117 1 0 1 1 0 0
EDGE2 36 17 1.0238 -0.0222964 0.0129001 1 0 1 1 0 0
EDGE2 37 36 -1.01533 -0.00862644 -0.0058424 1 0 1 1 0 0
EDGE2 37 16 -1.01359 -0.0613493 -0.0122542 1 0 1 1 0 0
EDGE2 37 18 1.03711 0.0426359 0.00482447 1 0 1 1 0 0
EDGE2 37 17 -0.0242366 0.0142009 -0.00533508 1 0 1 1 0 0
EDGE2 38 18 0.0345562 0.021662 -0.0131412 1 0 1 1 0 0
EDGE2 38 37 -0.981448 0.0523343 -0.0417572 1 0 1 1 0 0
EDGE2 38 17 -0.961596 0.00213889 -0.00047585 1 0 1 1 0 0
EDGE2 38 19 1.0594 -0.0430789 0.0123265 1 0 1 1 0 0
EDGE2 39 38 -1.04804 -0.036889 -0.0235853 1 0 1 1 0 0
EDGE2 39 18 -1.03632 -0.0536162 0.048128 1 0 1 1 0 0
EDGE2 39 19 0.0361672 -0.0534412 -0.00346497 1 0 1 1 0 0
EDGE2 39 0 1.00175 0.0276332 1.57543 1 0 1 1 0 0
EDGE2 39 20 0.964769 0.103526 -0.00735911 1 0 1 1 0 0
EDGE2 40 39 -1.03933 0.097229 0.0136057 1 0 1 1 0 0
EDGE2 40 19 -1.02893 -0.0692033 0.00855088 1 0 1 1 0 0
EDGE2 40 1 0.0574661 1.07674 1.54189 1 0 1 1 0 0
EDGE2 40 21 0.0696538 0.998274 1.58379 1 0 1 1 0 0
EDGE2 40 0 -0.03593 0.00262297 1.5732 1 0 1 1 0 0
EDGE2 40 20 0.0769078 -0.0463276 -0.0240456 1 0 1 1 0 0
EDGE2 41 40 -1.07316 0.00254372 -1.56145 1 0 1 1 0 0
EDGE2 41 2 0.97152 -0.00150084 -0.0165972 1 0 1 1 0 0
EDGE2 41 22 0.93936 -0.00378253 0.0077679 1 0 1 1 0 0
EDGE2 41 1 0.0512203 -0.0531063 -0.0128993 1 0 1 1 0 0
EDGE2 41 21 0.0969106 -0.00304953 -0.00537574 1 0 1 1 0 0
EDGE2 41 0 -1.0034 -0.025292 -0.00388085 1 0 1 1 0 0
EDGE2 41 20 -1.05749 0.0115896 -1.58133 1 0 1 1 0 0
EDGE2 42 2 0.0235949 0.00811449 -0.00358233 1 0 1 1 0 0
EDGE2 42 3 0.991071 -0.0286072 0.0236739 1 0 1 1 0 0
EDGE2 42 23 1.05311 0.0104005 -0.00518416 1 0 1 1 0 0
EDGE2 42 22 -0.0455836 0.0249413 -0.011739 1 0 1 1 0 0
EDGE2 42 1 -1.01015 -0.014061 -0.0169645 1 0 1 1 0 0
EDGE2 42 21 -0.892462 -0.0125915 0.0271967 1 0 1 1 0 0
EDGE2 42 41 -1.03813 0.0163744 0.00425822 1 0 1 1 0 0
EDGE2 43 24 0.932658 0.079441 0.00305436 1 0 1 1 0 0
EDGE2 43 4 1.04656 0.04523 -0.0109827 1 0 1 1 0 0
EDGE2 43 2 -1.01747 0.0488344 -0.00688551 1 0 1 1 0 0
EDGE2 43 42 -0.988066 -0.0104386 0.0502105 1 0 1 1 0 0
EDGE2 43 3 -0.0201045 -0.059931 0.0208538 1 0 1 1 0 0
EDGE2 43 23 0.0121089 0.0411716 0.0274525 1 0 1 1 0 0
EDGE2 43 22 -1.06541 0.00647669 -0.0152333 1 0 1 1 0 0
EDGE2 44 43 -1.06111 -0.0138941 -0.0151349 1 0 1 1 0 0
EDGE2 44 25 0.907841 -0.0585735 -0.0156694 1 0 1 1 0 0
EDGE2 44 24 0.080514 0.0102301 -0.000739134 1 0 1 1 0 0
EDGE2 44 5 0.994756 0.106482 -0.0329364 1 0 1 1 0 0
EDGE2 44 4 0.0336562 0.0350477 -0.0134209 1 0 1 1 0 0
EDGE2 44 3 -0.977117 0.0175205 0.0431175 1 0 1 1 0 0
EDGE2 44 23 -0.995286 0.0403712 0.0311094 1 0 1 1 0 0
EDGE2 45 25 0.086721 0.0210672 -0.01128 1 0 1 1 0 0
EDGE2 45 6 -0.00964881 0.917532 1.58991 1 0 1 1 0 0
EDGE2 45 26 -0.107945 1.00294 1.60674 1 0 1 1 0 0
EDGE2 45 24 -1.06401 0.0985536 0.00301256 1 0 1 1 0 0
EDGE2 45 44 -1.11052 0.0279259 0.0194036 1 0 1 1 0 0
EDGE2 45 5 0.00693003 -0.107152 0.0347982 1 0 1 1 0 0
EDGE2 45 4 -0.975055 0.00541385 0.00019082 1 0 1 1 0 0
EDGE2 46 27 0.957182 -0.0308347 0.0232143 1 0 1 1 0 0
EDGE2 46 7 1.08788 0.0127277 -0.00918705 1 0 1 1 0 0
EDGE2 46 25 -1.07359 0.0262712 -1.59817 1 0 1 1 0 0
EDGE2 46 6 0.0251908 0.0703469 -0.0114448 1 0 1 1 0 0
EDGE2 46 26 -0.0555992 -0.0169743 -0.0141156 1 0 1 1 0 0
EDGE2 46 45 -1.06391 0.0367663 -1.53 1 0 1 1 0 0
EDGE2 46 5 -0.997558 0.013946 -1.56515 1 0 1 1 0 0
EDGE2 47 27 -0.00659155 0.0206818 0.0308512 1 0 1 1 0 0
EDGE2 47 8 0.932297 0.0905773 0.0189001 1 0 1 1 0 0
EDGE2 47 28 1.01628 -0.0421346 0.06175 1 0 1 1 0 0
EDGE2 47 7 -0.0838918 0.100573 -0.00623043 1 0 1 1 0 0
EDGE2 47 6 -1.09254 -3.79076e-05 0.00973934 1 0 1 1 0 0
EDGE2 47 26 -0.969367 0.0109812 0.0193835 1 0 1 1 0 0
EDGE2 47 46 -0.968302 -0.0552186 0.0358645 1 0 1 1 0 0
EDGE2 48 29 1.07313 -0.148712 0.00196928 1 0 1 1 0 0
EDGE2 48 9 1.0849 0.0223434 0.00590472 1 0 1 1 0 0
EDGE2 48 27 -0.914726 -0.0647734 -0.0173613 1 0 1 1 0 0
EDGE2 48 8 0.0426816 -0.042649 -0.0177828 1 0 1 1 0 0
EDGE2 48 28 0.0379321 -0.0109102 -0.0102417 1 0 1 1 0 0
EDGE2 48 47 -1.06887 -0.0121303 -0.0489097 1 0 1 1 0 0
EDGE2 48 7 -0.965107 -0.034685 -0.000290141 1 0 1 1 0 0
EDGE2 49 10 1.12226 -0.0111233 -0.007791 1 0 1 1 0 0
EDGE2 49 30 0.989907 -0.0123868 0.019701 1 0 1 1 0 0
EDGE2 49 29 -0.0472499 0.0649252 -0.0229459 1 0 1 1 0 0
EDGE2 49 9 -0.0543329 0.00135195 0.015534 1 0 1 1 0 0
EDGE2 49 8 -0.989154 0.0177114 -0.00459804 1 0 1 1 0 0
EDGE2 49 28 -0.904061 0.0569324 0.0300751 1 0 1 1 0 0
EDGE2 49 48 -0.973676 0.051129 -0.0317589 1 0 1 1 0 0
EDGE2 50 31 0.0033669 0.916796 1.58905 1 0 1 1 0 0
EDGE2 50 10 -0.0446702 0.0139044 -0.0111686 1 0 1 1 0 0
EDGE2 50 30 0.000176756 0.0422134 0.00416723 1 0 1 1 0 0
EDGE2 50 11 0.0535292 0.975834 1.58092 1 0 1 1 0 0
EDGE2 50 29 -0.944412 -0.113794 -0.0170506 1 0 1 1 0 0
EDGE2 50 49 -1.02827 0.0163842 -0.0269791 1 0 1 1 0 0
EDGE2 50 9 -0.979417 -0.0710258 -0.00133216 1 0 1 1 0 0
EDGE2 51 31 -0.0518575 0.0249306 -0.00251684 1 0 1 1 0 0
EDGE2 51 10 -1.07854 0.0248414 -1.59534 1 0 1 1 0 0
EDGE2 51 30 -1.03347 0.0253831 -1.58701 1 0 1 1 0 0
EDGE2 51 50 -1.04958 0.0503268 -1.58131 1 0 1 1 0 0
EDGE2 51 32 1.00485 0.0028486 0.0195914 1 0 1 1 0 0
EDGE2 51 11 0.0349253 -0.0171134 0.00953402 1 0 1 1 0 0
EDGE2 51 12 1.00549 0.0628512 -0.0162726 1 0 1 1 0 0
EDGE2 52 31 -0.982801 -0.0303748 -0.00113303 1 0 1 1 0 0
EDGE2 52 51 -0.953712 -0.0847509 0.00541592 1 0 1 1 0 0
EDGE2 52 32 -0.0729728 -0.0432622 0.0151166 1 0 1 1 0 0
EDGE2 52 11 -0.994905 -0.0296327 -0.0114404 1 0 1 1 0 0
EDGE2 52 12 -0.108004 -0.0318606 0.0170936 1 0 1 1 0 0
EDGE2 52 33 0.918793 0.0285272 0.00925295 1 0 1 1 0 0
EDGE2 52 13 0.992951 0.0318294 -0.007191 1 0 1 1 0 0
EDGE2 53 32 -0.986992 -0.0026008 0.00451185 1 0 1 1 0 0
EDGE2 53 52 -0.960158 -0.0578602 0.0212446 1 0 1 1 0 0
EDGE2 53 12 -0.938384 0.0130893 0.0177479 1 0 1 1 0 0
EDGE2 53 33 -0.00487972 0.0802647 -0.0156472 1 0 1 1 0 0
EDGE2 53 13 -0.0429264 -0.0121861 0.0191245 1 0 1 1 0 0
EDGE2 53 14 0.972309 0.0374106 -0.0227226 1 0 1 1 0 0
EDGE2 53 34 0.920651 -0.0381205 0.00156026 1 0 1 1 0 0
EDGE2 54 35 1.02746 -0.0232173 -0.00733268 1 0 1 1 0 0
EDGE2 54 53 -1.03388 0.0605877 -0.0242274 1 0 1 1 0 0
EDGE2 54 33 -0.953977 -0.0238591 0.0112294 1 0 1 1 0 0
EDGE2 54 13 -1.01876 0.000850867 0.023767 1 0 1 1 0 0
EDGE2 54 14 0.0142706 0.0568535 0.0156199 1 0 1 1 0 0
EDGE2 54 34 0.029565 0.110446 -0.0176974 1 0 1 1 0 0
EDGE2 54 15 1.08373 0.101394 0.0117167 1 0 1 1 0 0
EDGE2 55 35 -0.0483156 -0.0176008 0.0190501 1 0 1 1 0 0
EDGE2 55 54 -1.05102 -0.0108082 -0.0129526 1 0 1 1 0 0
EDGE2 55 14 -1.00768 0.0938761 0.0063347 1 0 1 1 0 0
EDGE2 55 34 -1.01362 0.020014 -0.00713884 1 0 1 1 0 0
EDGE2 55 15 0.0386252 0.0479351 -0.000568738 1 0 1 1 0 0
EDGE2 55 36 -0.00378631 0.992718 1.59098 1 0 1 1 0 0
EDGE2 55 16 0.0886423 1.02733 1.62359 1 0 1 1 0 0
EDGE2 56 35 -0.974585 -0.0699549 -1.57271 1 0 1 1 0 0
EDGE2 56 55 -1.05595 -0.00465349 -1.60392 1 0 1 1 0 0
EDGE2 56 15 -0.928016 -0.00655726 -1.55364 1 0 1 1 0 0
EDGE2 56 36 0.0437675 -0.00458342 -0.011688 1 0 1 1 0 0
EDGE2 56 16 0.024268 -0.090309 -0.00528886 1 0 1 1 0 0
EDGE2 56 37 0.949795 -0.0460936 0.0143859 1 0 1 1 0 0
EDGE2 56 17 0.962006 0.0477689 -0.00212384 1 0 1 1 0 0
EDGE2 57 36 -1.08411 0.0570905 -0.015367 1 0 1 1 0 0
EDGE2 57 56 -0.983438 -0.000514897 -0.005883 1 0 1 1 0 0
EDGE2 57 16 -0.973679 0.00432404 -0.00761843 1 0 1 1 0 0
EDGE2 57 38 0.907871 -0.109092 -0.00238144 1 0 1 1 0 0
EDGE2 57 18 1.00501 0.0775972 0.00722838 1 0 1 1 0 0
EDGE2 57 37 0.00880416 -0.0508718 0.0130591 1 0 1 1 0 0
EDGE2 57 17 0.000245102 0.0241579 0.0127276 1 0 1 1 0 0
EDGE2 58 57 -0.991164 0.0516127 0.0222056 1 0 1 1 0 0
EDGE2 58 38 -0.0687829 0.0653377 -0.00943642 1 0 1 1 0 0
EDGE2 58 18 -0.0626875 -0.108306 -0.00969368 1 0 1 1 0 0
EDGE2 58 37 -0.941803 0.0279259 0.0445915 1 0 1 1 0 0
EDGE2 58 17 -1.05002 -0.0131603 -0.0219704 1 0 1 1 0 0
EDGE2 58 39 0.902028 -0.0456874 0.0293116 1 0 1 1 0 0
EDGE2 58 19 1.05936 0.100124 -0.010211 1 0 1 1 0 0
EDGE2 59 38 -1.02644 -0.0343746 0.00425898 1 0 1 1 0 0
EDGE2 59 58 -0.850528 -0.00749867 0.0421351 1 0 1 1 0 0
EDGE2 59 18 -0.885152 0.00873652 -0.00819438 1 0 1 1 0 0
EDGE2 59 39 -0.0649969 -0.0533229 -0.0304834 1 0 1 1 0 0
EDGE2 59 19 0.0817007 0.0632502 -0.00511583 1 0 1 1 0 0
EDGE2 59 40 1.00694 -0.114722 0.0220379 1 0 1 1 0 0
EDGE2 59 0 0.937874 0.0125331 1.55639 1 0 1 1 0 0
EDGE2 59 20 0.983498 0.0680913 -0.0101281 1 0 1 1 0 0
EDGE2 60 59 -1.0258 -0.02312 0.0206348 1 0 1 1 0 0
EDGE2 60 39 -0.871643 -0.0291038 0.00245518 1 0 1 1 0 0
EDGE2 60 19 -0.991389 -0.0174307 0.00265999 1 0 1 1 0 0
EDGE2 60 40 -0.0639512 0.0351714 0.00972188 1 0 1 1 0 0
EDGE2 60 1 0.0945945 0.903081 1.5947 1 0 1 1 0 0
EDGE2 60 21 -0.0395067 0.984867 1.57746 1 0 1 1 0 0
EDGE2 60 41 -0.0961783 1.01885 1.59479 1 0 1 1 0 0
EDGE2 60 0 0.0351455 -0.0494314 1.58969 1 0 1 1 0 0
EDGE2 60 20 -0.10896 0.00568799 -0.00742316 1 0 1 1 0 0
EDGE2 61 40 -1.02371 -0.0384713 1.56846 1 0 1 1 0 0
EDGE2 61 60 -1.00157 0.013005 1.56099 1 0 1 1 0 0
EDGE2 61 0 -0.999539 -0.0815131 -3.11522 1 0 1 1 0 0
EDGE2 61 20 -1.05417 -0.0432731 1.57241 1 0 1 1 0 0
EDGE2 62 61 -1.06614 -0.0518437 0.0210032 1 0 1 1 0 0
EDGE2 63 62 -1.00226 0.0602444 0.013051 1 0 1 1 0 0
EDGE2 64 63 -1.04961 0.0446695 0.0205524 1 0 1 1 0 0
EDGE2 65 64 -1.01538 -0.0917619 -0.00399046 1 0 1 1 0 0
EDGE2 66 65 -1.06092 -0.0548153 1.59301 1 0 1 1 0 0
EDGE2 67 66 -1.01683 -0.0120376 -0.0043823 1 0 1 1 0 0
EDGE2 68 67 -1.00712 -0.0263215 -0.00660172 1 0 1 1 0 0
EDGE2 69 68 -1.02908 0.012488 -0.0426752 1 0 1 1 0 0
EDGE2 70 69 -0.933982 0.0175617 -0.00804355 1 0 1 1 0 0
EDGE2 71 70 -0.951899 0.105287 -1.5595 1 0 1 1 0 0
EDGE2 72 71 -0.990263 -0.0891221 -0.0303282 1 0 1 1 0 0
EDGE2 73 72 -1.00818 0.0534858 0.0341473 1 0 1 1 0 0
EDGE2 74 73 -0.927587 0.0355366 0.0278978 1 0 1 1 0 0
EDGE2 75 74 -1.00835 0.0200616 -0.00325495 1 0 1 1 0 0
EDGE2 76 75 -0.911527 -0.0580256 -1.56211 1 0 1 1 0 0
EDGE2 77 76 -1.0866 -0.0563987 -0.0305906 1 0 1 1 0 0
EDGE2 78 77 -1.02909 0.0703149 0.0223702 1 0 1 1 0 0
EDGE2 79 78 -1.0709 0.00915469 0.0140786 1 0 1 1 0 0
EDGE2 80 79 -1.02792 0.0686446 0.027726 1 0 1 1 0 0
EDGE2 81 80 -0.959181 -0.0396398 -1.571 1 0 1 1 0 0
EDGE2 82 81 -1.04191 0.0577756 0.00427146 1 0 1 1 0 0
EDGE2 83 82 -0.936917 -0.0584888 0.00552186 1 0 1 1 0 0
EDGE2 84 65 0.951358 0.0212743 -3.15858 1 0 1 1 0 0
EDGE2 84 83 -1.01061 -0.0528576 -0.0203836 1 0 1 1 0 0
EDGE2 85 65 0.124357 0.0505873 -3.15701 1 0 1 1 0 0
EDGE2 85 66 -0.0270947 1.02244 1.57347 1 0 1 1 0 0
EDGE2 85 64 1.02493 0.016563 -3.16695 1 0 1 1 0 0
EDGE2 85 84 -0.987886 0.00451597 0.00269797 1 0 1 1 0 0
EDGE2 86 65 -1.02735 -0.0704468 -1.56691 1 0 1 1 0 0
EDGE2 86 85 -0.972762 -0.0528197 1.57225 1 0 1 1 0 0
EDGE2 87 86 -0.91883 0.0375303 -0.0123346 1 0 1 1 0 0
EDGE2 88 87 -1.08097 -0.104538 0.0061549 1 0 1 1 0 0
EDGE2 89 88 -1.00986 -0.0113462 -0.00742318 1 0 1 1 0 0
EDGE2 90 89 -1.03219 -0.0165154 -0.00181226 1 0 1 1 0 0
EDGE2 91 90 -0.940739 0.0240986 -1.5619 1 0 1 1 0 0
EDGE2 92 91 -1.15555 -0.0727788 -0.00855884 1 0 1 1 0 0
EDGE2 93 92 -1.04254 0.083779 0.034681 1 0 1 1 0 0
EDGE2 94 93 -1.07811 0.0709857 -0.00287562 1 0 1 1 0 0
EDGE2 95 94 -0.916162 0.0464945 -0.0109007 1 0 1 1 0 0
EDGE2 96 95 -0.983042 -0.10662 -1.57228 1 0 1 1 0 0
EDGE2 97 96 -0.933411 0.024514 0.0124052 1 0 1 1 0 0
EDGE2 98 97 -1.03384 0.0366015 -0.00108394 1 0 1 1 0 0
EDGE2 99 40 0.949815 -0.0200951 -3.15597 1 0 1 1 0 0
EDGE2 99 60 0.982217 0.038669 -3.1331 1 0 1 1 0 0
EDGE2 99 0 1.14879 0.0129747 -1.57804 1 0 1 1 0 0
EDGE2 99 20 1.00089 0.0504254 -3.15355 1 0 1 1 0 0
EDGE2 99 98 -1.00067 -0.053216 -0.0254751 1 0 1 1 0 0
EQUIV 21 1
EQUIV 22 2
EQUIV 23 3
EQUIV 24 4
EQUIV 25 5
EQUIV 26 6
EQUIV 27 7
EQUIV 28 8
EQUIV 29 9
EQUIV 30 10
EQUIV 31 11
EQUIV 32 12
EQUIV 33 13
EQUIV 34 14
EQUIV 35 15
EQUIV 36 16
EQUIV 37 17
EQUIV 38 18
EQUIV 39 19
EQUIV 40 20
EQUIV 41 1
EQUIV 42 2
EQUIV 43 3
EQUIV 44 4
EQUIV 45 5
EQUIV 46 6
EQUIV 47 7
EQUIV 48 8
EQUIV 49 9
EQUIV 50 10
EQUIV 51 11
EQUIV 52 12
EQUIV 53 13
EQUIV 54 14
EQUIV 55 15
EQUIV 56 16
EQUIV 57 17
EQUIV 58 18
EQUIV 59 19
EQUIV 60 20

80186
examples/Data/w10000-odom.graph Normal file
View File

File diff suppressed because it is too large Load Diff

119
examples/DiscreteBayesNet_FG.cpp Normal file
View File

@ -0,0 +1,119 @@
/* ----------------------------------------------------------------------------
* 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 DiscreteBayesNet_FG.cpp
* @brief Discrete Bayes Net example using Factor Graphs
* @author Abhijit
* @date Jun 4, 2012
*
* We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, p529]
* You may be familiar with other graphical model packages like BNT (available
* at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this is used as an
* example. The following demo is same as that in the above link, except that
* everything is using GTSAM.
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <iomanip>
using namespace std;
using namespace gtsam;
int main(int argc, char **argv) {
// We assume binary state variables
// we have 0 == "False" and 1 == "True"
const size_t nrStates = 2;
// define variables
DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates),
WetGrass(4, nrStates);
// create Factor Graph of the bayes net
DiscreteFactorGraph graph;
// add factors
graph.add(Cloudy, "0.5 0.5"); //P(Cloudy)
graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy)
graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy)
graph.add(Sprinkler & Rain & WetGrass,
"1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain)
// Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional
// factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp)
// Since this is a relatively small distribution, we can as well print
// the whole distribution..
cout << "Distribution of Example: " << endl;
cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10)
<< "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)"
<< endl;
for (size_t a = 0; a < nrStates; a++)
for (size_t m = 0; m < nrStates; m++)
for (size_t h = 0; h < nrStates; h++)
for (size_t c = 0; c < nrStates; c++) {
DiscreteFactor::Values values;
values[Cloudy.first] = c;
values[Sprinkler.first] = h;
values[Rain.first] = m;
values[WetGrass.first] = a;
double prodPot = graph(values);
cout << boolalpha << setw(8) << (bool) c << setw(14)
<< (bool) h << setw(12) << (bool) m << setw(13)
<< (bool) a << setw(16) << prodPot << endl;
}
// "Most Probable Explanation", i.e., configuration with largest value
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
cout <<"\nMost Probable Explanation (MPE):" << endl;
cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first]
<< " Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first]
<< " Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first]
<< " WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl;
// "Inference" We show an inference query like: probability that the Sprinkler was on;
// given that the grass is wet i.e. P( S | W=1) =?
cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl;
// Method 1: we can compute the joint marginal P(S,W) and from that we can compute
// P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps..
//Step1: Compute P(S,W)
DiscreteFactorGraph jointFG;
jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices());
DecisionTreeFactor probSW = jointFG.product();
//Step2: Compute P(W)
DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first);
//Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1)
DiscreteFactor::Values values;
values[WetGrass.first] = 1;
//print P(S=0|W=1)
values[Sprinkler.first] = 0;
cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl;
//print P(S=1|W=1)
values[Sprinkler.first] = 1;
cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl;
// TODO: Method 2 : One way is to modify the factor graph to
// incorporate the evidence node and compute the marginal
// TODO: graph.addEvidence(Cloudy,0);
return 0;
}

159
examples/LocalizationExample.cpp Normal file
View File

@ -0,0 +1,159 @@
/* ----------------------------------------------------------------------------
* 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 LocalizationExample.cpp
* @brief Simple robot localization example, with three "GPS-like" measurements
* @author Frank Dellaert
*/
/**
* A simple 2D pose slam example with "GPS" measurements
* - The robot moves forward 2 meter each iteration
* - The robot initially faces along the X axis (horizontal, to the right in 2D)
* - We have full odometry between pose
* - We have "GPS-like" measurements implemented with a custom factor
*/
// We will use Pose2 variables (x, y, theta) to represent the robot positions
#include <gtsam/geometry/Pose2.h>
// We will use simple integer Keys to refer to the robot poses.
#include <gtsam/nonlinear/Key.h>
// As in OdometryExample.cpp, we use a BetweenFactor to model odometry measurements.
#include <gtsam/slam/BetweenFactor.h>
// We add all facors to a Nonlinear Factor Graph, as our factors are nonlinear.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
// standard Levenberg-Marquardt solver
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
// Before we begin the example, we must create a custom unary factor to implement a
// "GPS-like" functionality. Because standard GPS measurements provide information
// only on the position, and not on the orientation, we cannot use a simple prior to
// properly model this measurement.
//
// The factor will be a unary factor, affect only a single system variable. It will
// also use a standard Gaussian noise model. Hence, we will derive our new factor from
// the NoiseModelFactor1.
#include <gtsam/nonlinear/NonlinearFactor.h>
class UnaryFactor: public NoiseModelFactor1<Pose2> {
// The factor will hold a measurement consisting of an (X,Y) location
// We could this with a Point2 but here we just use two doubles
double mx_, my_;
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<UnaryFactor> shared_ptr;
// The constructor requires the variable key, the (X, Y) measurement value, and the noise model
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
virtual ~UnaryFactor() {}
// Using the NoiseModelFactor1 base class there are two functions that must be overridden.
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
{
// The measurement function for a GPS-like measurement is simple:
// error_x = pose.x - measurement.x
// error_y = pose.y - measurement.y
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0);
return Vector_(2, q.x() - mx_, q.y() - my_);
}
// The second is a 'clone' function that allows the factor to be copied. Under most
// circumstances, the following code that employs the default copy constructor should
// work fine.
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
// Additionally, we encourage you the use of unit testing your custom factors,
// (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
// GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
}; // UnaryFactor
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 10cm std on x,y
graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize using Levenberg-Marquardt optimization. The optimizer
// accepts an optional set of configuration parameters, controlling
// things like convergence criteria, the type of linear system solver
// to use, and the amount of information displayed during optimization.
// Here we will use the default set of parameters. See the
// documentation for the full set of parameters.
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
result.print("Final Result:\n");
// 5. Calculate and print marginal covariances for all variables
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}

99
examples/OdometryExample.cpp Normal file
View File

@ -0,0 +1,99 @@
/* ----------------------------------------------------------------------------
* 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 OdometryExample.cpp
* @brief Simple robot motion example, with prior and two odometry measurements
* @author Frank Dellaert
*/
/**
* Example of a simple 2D localization example
* - Robot poses are facing along the X axis (horizontal, to the right in 2D)
* - The robot moves 2 meters each step
* - We have full odometry between poses
*/
// We will use Pose2 variables (x, y, theta) to represent the robot positions
#include <gtsam/geometry/Pose2.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
// Levenberg-Marquardt solver
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Create an empty nonlinear factor graph
NonlinearFactorGraph graph;
// Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
initial.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("Final Result:\n");
// Calculate and print marginal covariances for all variables
cout.precision(2);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}

142
examples/PlanarSLAMExample.cpp Normal file
View File

@ -0,0 +1,142 @@
/* ----------------------------------------------------------------------------
* 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 PlanarSLAMExample.cpp
* @brief Simple robotics example using odometry measurements and bearing-range (laser) measurements
* @author Alex Cunningham
*/
/**
* A simple 2D planar slam example with landmarks
* - The robot and landmarks are on a 2 meter grid
* - Robot poses are facing along the X axis (horizontal, to the right in 2D)
* - The robot moves 2 meters each step
* - We have full odometry between poses
* - We have bearing and range information for measurements
* - Landmarks are 2 meters away from the robot trajectory
*/
// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent
// the robot positions and Point2 variables (x, y) to represent the landmark coordinates.
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use a RangeBearing factor for the range-bearing measurements to identified
// landmarks, and Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
// common Levenberg-Marquardt solver
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Create a factor graph
NonlinearFactorGraph graph;
// Create the keys we need for this simple example
static Symbol x1('x',1), x2('x',2), x3('x',3);
static Symbol l1('l',1), l2('l',2);
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = std::sqrt(4.0+4.0),
range21 = 2.0,
range32 = 2.0;
// Add Bearing-Range factors
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
// Print
graph.print("Factor Graph:\n");
// Create (deliberately inaccurate) initial estimate
Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8));
// Print
initialEstimate.print("Initial Estimate:\n");
// Optimize using Levenberg-Marquardt optimization. The optimizer
// accepts an optional set of configuration parameters, controlling
// things like convergence criteria, the type of linear system solver
// to use, and the amount of information displayed during optimization.
// Here we will use the default set of parameters. See the
// documentation for the full set of parameters.
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
result.print("Final Result:\n");
// Calculate and print marginal covariances for all variables
Marginals marginals(graph, result);
print(marginals.marginalCovariance(x1), "x1 covariance");
print(marginals.marginalCovariance(x2), "x2 covariance");
print(marginals.marginalCovariance(x3), "x3 covariance");
print(marginals.marginalCovariance(l1), "l1 covariance");
print(marginals.marginalCovariance(l2), "l2 covariance");
return 0;
}

91
examples/PlanarSLAMExample_easy.cpp
View File

@ -1,91 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 PlanarSLAMExample.cpp
* @brief Simple robotics example using the pre-built planar SLAM domain
* @author Alex Cunningham
*/
#include <cmath>
#include <iostream>
// pull in the planar SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
using namespace planarSLAM;
/**
* In this version of the system we make the following assumptions:
* - All values are axis aligned
* - Robot poses are facing along the X axis (horizontal, to the right in images)
* - We have bearing and range information for measurements
* - We have full odometry for measurements
* - The robot and landmarks are on a grid, moving 2 meters each step
* - Landmarks are 2 meters away from the robot trajectory
*/
int main(int argc, char** argv) {
// create graph container and add factors to it
Graph graph;
/* add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* add odometry */
// general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
/* add measurements */
// general noisemodel for measurements
SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2));
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4),
range21 = 2.0,
range32 = 2.0;
// create bearing/range factors and add them
graph.addBearingRange(1, 1, bearing11, range11, meas_model);
graph.addBearingRange(2, 1, bearing21, range21, meas_model);
graph.addBearingRange(3, 2, bearing32, range32, meas_model);
graph.print("full graph");
// initialize to noisy points
planarSLAM::Values initialEstimate;
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(1, Point2(1.8, 2.1));
initialEstimate.insertPoint(2, Point2(4.1, 1.8));
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
planarSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
result.print("final result");
return 0;
}

137
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -1,137 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 PlanarSLAMSelfContained_advanced.cpp
* @brief Simple robotics example with all typedefs internal to this script.
* @author Alex Cunningham
*/
#include <cmath>
#include <iostream>
// for all nonlinear keys
#include <gtsam/nonlinear/Symbol.h>
// for points and poses
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
// for modeling measurement uncertainty - all models included here
#include <gtsam/linear/NoiseModel.h>
// add in headers for specific factors
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
/**
* In this version of the system we make the following assumptions:
* - All values are axis aligned
* - Robot poses are facing along the X axis (horizontal, to the right in images)
* - We have bearing and range information for measurements
* - We have full odometry for measurements
* - The robot and landmarks are on a grid, moving 2 meters each step
* - Landmarks are 2 meters away from the robot trajectory
*/
int main(int argc, char** argv) {
// create keys for variables
Symbol x1('x',1), x2('x',2), x3('x',3);
Symbol l1('l',1), l2('l',2);
// create graph container and add factors to it
NonlinearFactorGraph graph;
/* add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
PriorFactor<Pose2> posePrior(x1, prior_measurement, prior_model); // create the factor
graph.add(posePrior); // add the factor to the graph
/* add odometry */
// general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry
BetweenFactor<Pose2> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Pose2> odom23(x2, x3, odom_measurement, odom_model);
graph.add(odom12); // add both to graph
graph.add(odom23);
/* add measurements */
// general noisemodel for measurements
SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2));
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4),
range21 = 2.0,
range32 = 2.0;
// create bearing/range factors
BearingRangeFactor<Pose2, Point2> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Pose2, Point2> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Pose2, Point2> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors
graph.add(meas11);
graph.add(meas21);
graph.add(meas32);
graph.print("Full Graph");
// initialize to noisy points
Values initial;
initial.insert(x1, Pose2(0.5, 0.0, 0.2));
initial.insert(x2, Pose2(2.3, 0.1,-0.2));
initial.insert(x3, Pose2(4.1, 0.1, 0.1));
initial.insert(l1, Point2(1.8, 2.1));
initial.insert(l2, Point2(4.1, 1.8));
initial.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
// first using sequential elimination
LevenbergMarquardtParams lmParams;
lmParams.elimination = LevenbergMarquardtParams::SEQUENTIAL;
Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
resultSequential.print("final result (solved with a sequential solver)");
// then using multifrontal, advanced interface
// Note that we keep the original optimizer object so we can use the COLAMD
// ordering it computes.
LevenbergMarquardtOptimizer optimizer(graph, initial);
Values resultMultifrontal = optimizer.optimize();
resultMultifrontal.print("final result (solved with a multifrontal solver)");
// Print marginals covariances for all variables
Marginals marginals(graph, resultMultifrontal, Marginals::CHOLESKY);
print(marginals.marginalCovariance(x1), "x1 covariance");
print(marginals.marginalCovariance(x2), "x2 covariance");
print(marginals.marginalCovariance(x3), "x3 covariance");
print(marginals.marginalCovariance(l1), "l1 covariance");
print(marginals.marginalCovariance(l2), "l2 covariance");
return 0;
}

130
examples/Pose2SLAMExample.cpp Normal file
View File

@ -0,0 +1,130 @@
/* ----------------------------------------------------------------------------
* 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.cpp
* @brief A 2D Pose SLAM example
* @date Oct 21, 2010
* @author Yong Dian Jian
*/
/**
* A simple 2D pose slam example
* - The robot moves in a 2 meter square
* - The robot moves 2 meters each step, turning 90 degrees after each step
* - The robot initially faces along the X axis (horizontal, to the right in 2D)
* - We have full odometry between pose
* - We have a loop closure constraint when the robot returns to the first position
*/
// In planar SLAM example we use Pose2 variables (x, y, theta) to represent the robot poses
#include <gtsam/geometry/Pose2.h>
// We will use simple integer Keys to refer to the robot poses.
#include <gtsam/nonlinear/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
// a Gauss-Newton solver
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images. We will use another Between Factor to enforce this constraint:
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
// The optimizer accepts an optional set of configuration parameters,
// controlling things like convergence criteria, the type of linear
// system solver to use, and the amount of information displayed during
// optimization. We will set a few parameters as a demonstration.
GaussNewtonParams parameters;
// Stop iterating once the change in error between steps is less than this value
parameters.relativeErrorTol = 1e-5;
// Do not perform more than N iteration steps
parameters.maxIterations = 100;
// Create the optimizer ...
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
// ... and optimize
Values result = optimizer.optimize();
result.print("Final Result:\n");
// 5. Calculate and print marginal covariances for all variables
cout.precision(3);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl;
cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl;
return 0;
}

88
examples/Pose2SLAMExample_advanced.cpp
View File

@ -1,88 +0,0 @@
/* ----------------------------------------------------------------------------
* 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_advanced.cpp
* @brief Simple Pose2SLAM Example using
* pre-built pose2SLAM domain
* @author Chris Beall
*/
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace pose2SLAM;
int main(int argc, char** argv) {
/* 1. create graph container and add factors to it */
Graph graph;
/* 2.a add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* 2.b add odometry */
// general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */
pose2SLAM::Values initial;
initial.insertPose(1, Pose2(0.5, 0.0, 0.2));
initial.insertPose(2, Pose2(2.3, 0.1,-0.2));
initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
initial.print("initial estimate");
/* 4.2.1 Alternatively, you can go through the process step by step
* Choose an ordering */
Ordering ordering = *graph.orderingCOLAMD(initial);
/* 4.2.2 set up solver and optimize */
LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-15;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
pose2SLAM::Values result = optimizer.optimize();
result.print("final result");
/* Get covariances */
Marginals marginals(graph, result, Marginals::CHOLESKY);
Matrix covariance1 = marginals.marginalCovariance(PoseKey(1));
Matrix covariance2 = marginals.marginalCovariance(PoseKey(2));
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");
return 0;
}

69
examples/Pose2SLAMExample_easy.cpp
View File

@ -1,69 +0,0 @@
/* ----------------------------------------------------------------------------
* 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_easy.cpp
*
* A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
*
* @date Oct 21, 2010
* @author ydjian
*/
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
using namespace pose2SLAM;
int main(int argc, char** argv) {
/* 1. create graph container and add factors to it */
Graph graph ;
/* 2.a add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* 2.b add odometry */
// general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estinmate to the solution
* initialize to noisy points */
pose2SLAM::Values initial;
initial.insertPose(1, Pose2(0.5, 0.0, 0.2));
initial.insertPose(2, Pose2(2.3, 0.1,-0.2));
initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
initial.print("initial estimate");
/* 4 Single Step Optimization
* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
pose2SLAM::Values result = graph.optimize(initial);
result.print("final result");
return 0;
}

57
examples/Pose2SLAMExample_graph.cpp Normal file
View File

@ -0,0 +1,57 @@
/* ----------------------------------------------------------------------------
* 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/PriorFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Pose2.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
NonlinearFactorGraph::shared_ptr graph ;
Values::shared_ptr initial;
SharedDiagonal model = noiseModel::Diagonal::Sigmas(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 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01));
graph->add(PriorFactor<Pose2>(0, priorMean, priorNoise));
// Single Step Optimization using Levenberg-Marquardt
Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();
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;
}

118
examples/Pose2SLAMwSPCG.cpp Normal file
View File

@ -0,0 +1,118 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMwSPCG.cpp
* @brief A 2D Pose SLAM example using the SimpleSPCGSolver.
* @author Yong-Dian Jian
* @date June 2, 2012
*/
/**
* A simple 2D pose slam example solved using a Conjugate-Gradient method
* - The robot moves in a 2 meter square
* - The robot moves 2 meters each step, turning 90 degrees after each step
* - The robot initially faces along the X axis (horizontal, to the right in 2D)
* - We have full odometry between pose
* - We have a loop closure constraint when the robot returns to the first position
*/
// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent
// the robot positions
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// Each variable in the system (poses) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use simple integer keys
#include <gtsam/nonlinear/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, prior, priorNoise));
// 2b. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images.
// We will use another Between Factor to enforce this constraint, with the distance set to zero,
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Single Step Optimization using Levenberg-Marquardt
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
cout << "subgraph solver final error = " << graph.error(result) << endl;
}
return 0;
}

109
examples/Pose2SLAMwSPCG_advanced.cpp
View File

@ -1,109 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMwSPCG_advanced.cpp
* @brief Solve a simple 3 by 3 grid of Pose2 SLAM problem by using advanced SPCG interface
* @author Yong Dian
* Created October 21, 2010
*/
#include <iostream>
#if ENABLE_SPCG
#include <boost/shared_ptr.hpp>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
using namespace std;
using namespace gtsam;
using namespace pose2SLAM;
typedef boost::shared_ptr<Graph> sharedGraph ;
typedef boost::shared_ptr<Values> sharedValue ;
//typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
typedef boost::shared_ptr<Solver> sharedSolver ;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer;
sharedGraph graph;
sharedValue initial;
Values result;
/* ************************************************************************* */
int main(void) {
/* generate synthetic data */
const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1));
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
graph = boost::make_shared<Graph>() ;
initial = boost::make_shared<Values>() ;
// create a 3 by 3 grid
// x3 x6 x9
// x2 x5 x8
// x1 x4 x7
graph->addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
graph->addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
graph->addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
graph->addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
graph->addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
graph->addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
graph->addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
graph->addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
graph->addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
graph->addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
graph->addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
graph->addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
graph->addPrior(x1, Pose2(0,0,0), sigma) ;
initial->insert(x1, Pose2( 0, 0, 0));
initial->insert(x2, Pose2( 0, 2.1, 0.01));
initial->insert(x3, Pose2( 0, 3.9,-0.01));
initial->insert(x4, Pose2(2.1,-0.1, 0));
initial->insert(x5, Pose2(1.9, 2.1, 0.02));
initial->insert(x6, Pose2(2.0, 3.9,-0.02));
initial->insert(x7, Pose2(4.0, 0.1, 0.03 ));
initial->insert(x8, Pose2(3.9, 2.1, 0.01));
initial->insert(x9, Pose2(4.1, 3.9,-0.01));
/* done with generating data */
graph->print("full graph") ;
initial->print("initial estimate") ;
sharedSolver solver(new Solver(*graph, *initial)) ;
SPCGOptimizer optimizer(graph, initial, solver->ordering(), solver) ;
cout << "before optimization, sum of error is " << optimizer.error() << endl;
SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt();
cout << "after optimization, sum of error is " << optimizer2.error() << endl;
result = *optimizer2.values() ;
result.print("final result") ;
return 0 ;
}
#else
int main() {
std::cout << "SPCG is currently disabled" << std::endl;
return 0;
}
#endif

87
examples/Pose2SLAMwSPCG_easy.cpp
View File

@ -1,87 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMwSPCG_easy.cpp
* @brief Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
* @author Yong Dian
* Created October 21, 2010
*/
#include <iostream>
#if ENABLE_SPCG
#include <boost/shared_ptr.hpp>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
using namespace std;
using namespace gtsam;
using namespace pose2SLAM;
Graph graph;
Values initial, result;
/* ************************************************************************* */
int main(void) {
/* generate synthetic data */
const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1));
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
// create a 3 by 3 grid
// x3 x6 x9
// x2 x5 x8
// x1 x4 x7
graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
graph.addPrior(x1, Pose2(0,0,0), sigma) ;
initial.insert(x1, Pose2( 0, 0, 0));
initial.insert(x2, Pose2( 0, 2.1, 0.01));
initial.insert(x3, Pose2( 0, 3.9,-0.01));
initial.insert(x4, Pose2(2.1,-0.1, 0));
initial.insert(x5, Pose2(1.9, 2.1, 0.02));
initial.insert(x6, Pose2(2.0, 3.9,-0.02));
initial.insert(x7, Pose2(4.0, 0.1, 0.03 ));
initial.insert(x8, Pose2(3.9, 2.1, 0.01));
initial.insert(x9, Pose2(4.1, 3.9,-0.01));
/* done */
graph.print("full graph") ;
initial.print("initial estimate");
result = optimizeSPCG(graph, initial);
result.print("final result") ;
return 0 ;
}
#else
int main() {
std::cout << "SPCG is currently disabled" << std::endl;
return 0;
}
#endif

12
examples/README
View File

@ -1,4 +1,4 @@
This directory contains a number of exapmples that illustrate the use of GTSAM:
This directory contains a number of examples that illustrate the use of GTSAM:
SimpleRotation: a super-simple example of optimizing a single rotation according to a single prior
@ -11,18 +11,20 @@ errorStateKalmanFilter: simple 1D example of a moving target measured by a accel
2D Pose SLAM
============
Pose2SLAMExample_easy: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
LocalizationExample.cpp: modeling robot motion
LocalizationExample2.cpp: example with GPS like measurements
Pose2SLAMExample: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
Pose2SLAMExample_advanced: same, but uses an Optimizer object
Pose2SLAMwSPCG_easy: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
Pose2SLAMwSPCG_advanced: solve a simple 3 by 3 grid of Pose2 SLAM problem by using advanced SPCG interface
Pose2SLAMwSPCG: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
Planar SLAM with landmarks
==========================
PlanarSLAMExample: simple robotics example using the pre-built planar SLAM domain
PlanarSLAMSelfContained_advanced: simple robotics example with all typedefs internal to this script.
PlanarSLAMExample_selfcontained: simple robotics example with all typedefs internal to this script.
Visual SLAM
===========
CameraResectioning.cpp: An example of gtsam for solving the camera resectioning problem
The directory vSLAMexample includes 2 simple examples using GTSAM:
- vSFMexample using visualSLAM in for structure-from-motion (SFM), and
- vISAMexample using visualSLAM and ISAM for incremental SLAM updates

54
examples/SimpleRotation.cpp
View File

@ -17,20 +17,41 @@
* @author Alex Cunningham
*/
#include <cmath>
#include <iostream>
#include <gtsam/slam/PriorFactor.h>
/**
* This example will perform a relatively trivial optimization on
* a single variable with a single factor.
*/
// In this example, a 2D rotation will be used as the variable of interest
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h>
// Each variable in the system (poses) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use symbols
#include <gtsam/nonlinear/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// We will apply a simple prior on the rotation
#include <gtsam/slam/PriorFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
// standard Levenberg-Marquardt solver
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
/*
* TODO: make factors independent of RotValues
* TODO: make toplevel documentation
* TODO: Clean up nonlinear optimization API
*/
using namespace std;
using namespace gtsam;
@ -40,12 +61,7 @@ const double degree = M_PI / 180;
int main() {
/**
* This example will perform a relatively trivial optimization on
* a single variable with a single factor.
*/
/**
* Step 1: create a factor on to express a unary constraint
* Step 1: Create a factor to express a unary constraint
* The "prior" in this case is the measurement from a sensor,
* with a model of the noise on the measurement.
*
@ -60,12 +76,12 @@ int main() {
*/
Rot2 prior = Rot2::fromAngle(30 * degree);
prior.print("goal angle");
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Symbol key('x',1);
PriorFactor<Rot2> factor(key, prior, model);
/**
* Step 2: create a graph container and add the factor to it
* Step 2: Create a graph container and add the factor to it
* Before optimizing, all factors need to be added to a Graph container,
* which provides the necessary top-level functionality for defining a
* system of constraints.
@ -78,7 +94,7 @@ int main() {
graph.print("full graph");
/**
* Step 3: create an initial estimate
* Step 3: Create an initial estimate
* An initial estimate of the solution for the system is necessary to
* start optimization. This system state is the "RotValues" structure,
* which is similar in structure to a STL map, in that it maps
@ -98,7 +114,7 @@ int main() {
initial.print("initial estimate");
/**
* Step 4: optimize
* Step 4: Optimize
* After formulating the problem with a graph of constraints
* and an initial estimate, executing optimization is as simple
* as calling a general optimization function with the graph and

114
examples/UGM_chain.cpp Normal file
View File

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* 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 small.cpp
* @brief UGM (undirected graphical model) examples: chain
* @author Frank Dellaert
* @author Abhijit Kundu
*
* See http://www.di.ens.fr/~mschmidt/Software/UGM/chain.html
* for more explanation. This code demos the same example using GTSAM.
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/base/timing.h>
#include <iomanip>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Set Number of Nodes in the Graph
const int nrNodes = 60;
// Each node takes 1 of 7 possible states denoted by 0-6 in following order:
// ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)"
// "Industry(with PhD)" "Academia" "Deceased"]
const size_t nrStates = 7;
// define variables
vector<DiscreteKey> nodes;
for (int i = 0; i < nrNodes; i++){
DiscreteKey dk(i, nrStates);
nodes.push_back(dk);
}
// create graph
DiscreteFactorGraph graph;
// add node potentials
graph.add(nodes[0], ".3 .6 .1 0 0 0 0");
for (int i = 1; i < nrNodes; i++)
graph.add(nodes[i], "1 1 1 1 1 1 1");
const std::string edgePotential = ".08 .9 .01 0 0 0 .01 "
".03 .95 .01 0 0 0 .01 "
".06 .06 .75 .05 .05 .02 .01 "
"0 0 0 .3 .6 .09 .01 "
"0 0 0 .02 .95 .02 .01 "
"0 0 0 .01 .01 .97 .01 "
"0 0 0 0 0 0 1";
// add edge potentials
for (int i = 0; i < nrNodes - 1; i++)
graph.add(nodes[i] & nodes[i + 1], edgePotential);
cout << "Created Factor Graph with " << nrNodes << " variable nodes and "
<< graph.size() << " factors (Unary+Edge).";
// "Decoding", i.e., configuration with largest value
// We use sequential variable elimination
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n");
// "Inference" Computing marginals for each node
cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl;
tic_(1, "Sequential");
for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
++itr) {
//Compute the marginal
Vector margProbs = solver.marginalProbabilities(*itr);
//Print the marginals
cout << "Node#" << setw(4) << itr->first << " : ";
print(margProbs);
}
toc_(1, "Sequential");
// Here we'll make use of DiscreteMarginals class, which makes use of
// bayes-tree based shortcut evaluation of marginals
DiscreteMarginals marginals(graph);
cout << "\nComputing Node Marginals ..(BayesTree based)" << endl;
tic_(2, "Multifrontal");
for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
++itr) {
//Compute the marginal
Vector margProbs = marginals.marginalProbabilities(*itr);
//Print the marginals
cout << "Node#" << setw(4) << itr->first << " : ";
print(margProbs);
}
toc_(2, "Multifrontal");
tictoc_print_();
return 0;
}

40
examples/UGM_small.cpp
View File

@ -49,26 +49,38 @@ int main(int argc, char** argv) {
// Print the UGM distribution
cout << "\nUGM distribution:" << endl;
for (size_t a = 0; a < nrStates; a++)
for (size_t m = 0; m < nrStates; m++)
for (size_t h = 0; h < nrStates; h++)
for (size_t c = 0; c < nrStates; c++) {
DiscreteFactor::Values values;
values[1] = c;
values[2] = h;
values[3] = m;
values[4] = a;
double prodPot = graph(values);
cout << c << " " << h << " " << m << " " << a << " :\t"
<< prodPot << "\t" << prodPot/3790 << endl;
}
vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
Cathy & Heather & Mark & Allison);
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteFactor::Values values = allPosbValues[i];
double prodPot = graph(values);
cout << values[Cathy.first] << " " << values[Heather.first] << " "
<< values[Mark.first] << " " << values[Allison.first] << " :\t"
<< prodPot << "\t" << prodPot / 3790 << endl;
}
// "Decoding", i.e., configuration with largest value
// "Decoding", i.e., configuration with largest value (MPE)
// We use sequential variable elimination
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
optimalDecoding->print("\noptimalDecoding");
// "Inference" Computing marginals
cout << "\nComputing Node Marginals .." << endl;
Vector margProbs;
margProbs = solver.marginalProbabilities(Cathy);
print(margProbs, "Cathy's Node Marginal:");
margProbs = solver.marginalProbabilities(Heather);
print(margProbs, "Heather's Node Marginal");
margProbs = solver.marginalProbabilities(Mark);
print(margProbs, "Mark's Node Marginal");
margProbs = solver.marginalProbabilities(Allison);
print(margProbs, "Allison's Node Marginal");
return 0;
}

159
examples/VisualISAM2Example.cpp Normal file
View File

@ -0,0 +1,159 @@
/* ----------------------------------------------------------------------------
* 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 VisualISAM2Example.cpp
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
* This version uses iSAM2 to solve the problem incrementally
* @author Duy-Nguyen Ta
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
// We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/SimpleCamera.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so
// include iSAM2 here
#include <gtsam/nonlinear/ISAM2.h>
// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
// and initial guesses for any new variables used in the added factors
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <vector>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
std::vector<gtsam::Point3> points;
points.push_back(gtsam::Point3(10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
// Create the set of ground-truth poses
std::vector<gtsam::Pose3> poses;
double radius = 30.0;
int i = 0;
double theta = 0.0;
gtsam::Point3 up(0,0,1);
gtsam::Point3 target(0,0,0);
for(; i < 8; ++i, theta += 2*M_PI/8) {
gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0);
gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
poses.push_back(camera.pose());
}
// Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization
// and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter
// structure is available that allows the user to set various properties, such as the relinearization threshold
// and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result
// will approach the batch result.
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
ISAM2 isam(parameters);
// Create a Factor Graph and Values to hold the new data
NonlinearFactorGraph graph;
Values initialEstimate;
// Loop over the different poses, adding the observations to iSAM incrementally
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
// If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale
// Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
// adding it to iSAM.
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
} else {
// Update iSAM with the new factors
isam.update(graph, initialEstimate);
// Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
// If accuracy is desired at the expense of time, update(*) can be called additional times
// to perform multiple optimizer iterations every step.
isam.update();
Values currentEstimate = isam.calculateEstimate();
cout << "****************************************************" << endl;
cout << "Frame " << i << ": " << endl;
currentEstimate.print("Current estimate: ");
// Clear the factor graph and values for the next iteration
graph.resize(0);
initialEstimate.clear();
}
}
return 0;
}
/* ************************************************************************* */

149
examples/VisualISAMExample.cpp Normal file
View File

@ -0,0 +1,149 @@
/* ----------------------------------------------------------------------------
* 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 VisualISAMExample.cpp
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
* This version uses iSAM to solve the problem incrementally
* @author Duy-Nguyen Ta
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
// We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/SimpleCamera.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// We want to use iSAM to solve the structure-from-motion problem incrementally, so
// include iSAM here
#include <gtsam/nonlinear/NonlinearISAM.h>
// iSAM requires as input a set set of new factors to be added stored in a factor graph,
// and initial guesses for any new variables used in the added factors
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <vector>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
std::vector<gtsam::Point3> points;
points.push_back(gtsam::Point3(10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
// Create the set of ground-truth poses
std::vector<gtsam::Pose3> poses;
double radius = 30.0;
int i = 0;
double theta = 0.0;
gtsam::Point3 up(0,0,1);
gtsam::Point3 target(0,0,0);
for(; i < 8; ++i, theta += 2*M_PI/8) {
gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0);
gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
poses.push_back(camera.pose());
}
// Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates
int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval);
// Create a Factor Graph and Values to hold the new data
NonlinearFactorGraph graph;
Values initialEstimate;
// Loop over the different poses, adding the observations to iSAM incrementally
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
// If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale
// Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
// adding it to iSAM.
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
} else {
// Update iSAM with the new factors
isam.update(graph, initialEstimate);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
cout << "Frame " << i << ": " << endl;
currentEstimate.print("Current estimate: ");
// Clear the factor graph and values for the next iteration
graph.resize(0);
initialEstimate.clear();
}
}
return 0;
}
/* ************************************************************************* */

139
examples/VisualSLAMExample.cpp Normal file
View File

@ -0,0 +1,139 @@
/* ----------------------------------------------------------------------------
* 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 VisualSLAMExample.cpp
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
* @author Duy-Nguyen Ta
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
// We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/SimpleCamera.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
// trust-region method known as Powell's Degleg
#include <gtsam/nonlinear/DoglegOptimizer.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <vector>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
std::vector<gtsam::Point3> points;
points.push_back(gtsam::Point3(10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
// Create the set of ground-truth poses
std::vector<gtsam::Pose3> poses;
double radius = 30.0;
int i = 0;
double theta = 0.0;
gtsam::Point3 up(0,0,1);
gtsam::Point3 target(0,0,0);
for(; i < 8; ++i, theta += 2*M_PI/8) {
gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0);
gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
poses.push_back(camera.pose());
}
// Create a factor graph
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
}
// Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initialEstimate estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n");
return 0;
}
/* ************************************************************************* */

3
examples/easyPoint2KalmanFilter.cpp
View File

@ -21,7 +21,8 @@
* @author Stephen Williams
*/
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Point2.h>

75
examples/matlab/PlanarSLAMExample_easy.m
View File

@ -1,75 +0,0 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Simple robotics example using the pre-built planar SLAM domain
% @author Alex Cunningham
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Assumptions
% - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images)
% - We have bearing and range information for measurements
% - We have full odometry for measurements
% - The robot and landmarks are on a grid, moving 2 meters each step
% - Landmarks are 2 meters away from the robot trajectory
%% Create keys for variables
x1 = 1; x2 = 2; x3 = 3;
l1 = 1; l2 = 2;
%% Create graph container and add factors to it
graph = planarSLAMGraph;
%% Add prior
% gaussian for prior
prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
%% Add measurements
% general noisemodel for measurements
meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
% create the measurement values - indices are (pose id, landmark id)
degrees = pi/180;
bearing11 = gtsamRot2(45*degrees);
bearing21 = gtsamRot2(90*degrees);
bearing32 = gtsamRot2(90*degrees);
range11 = sqrt(4+4);
range21 = 2.0;
range32 = 2.0;
% % create bearing/range factors and add them
graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
% print
graph.print('full graph');
%% Initialize to noisy points
initialEstimate = planarSLAMValues;
initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(l1, gtsamPoint2(1.8, 2.1));
initialEstimate.insertPoint(l2, gtsamPoint2(4.1, 1.8));
initialEstimate.print('initial estimate');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result.print('final result');

57
examples/matlab/Pose2SLAMExample_easy.m
View File

@ -1,57 +0,0 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Simple robotics example using the pre-built planar SLAM domain
% @author Alex Cunningham
% @author Frank Dellaert
% @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Assumptions
% - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images)
% - We have full odometry for measurements
% - The robot is on a grid, moving 2 meters each step
%% Create keys for variables
x1 = 1; x2 = 2; x3 = 3;
%% Create graph container and add factors to it
graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
%% Add measurements
% general noisemodel for measurements
meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
% print
graph.print('full graph');
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print('initial estimate');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result.print('final result');

14
examples/vSLAMexample/CMakeLists.txt
View File

@ -1,14 +0,0 @@
# Build vSLAMexample
message(STATUS "Adding Example vISAMexample")
add_executable(vISAMexample vISAMexample.cpp vSLAMutils.cpp)
target_link_libraries(vISAMexample gtsam-static)
add_dependencies(examples vISAMexample)
message(STATUS "Adding Example vSFMexample")
add_executable(vSFMexample vSFMexample.cpp vSLAMutils.cpp)
target_link_libraries(vSFMexample gtsam-static)
add_dependencies(examples vSFMexample)

41
examples/vSLAMexample/Feature2D.h
View File

@ -1,41 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Feature2D.h
* @brief
* @author Duy-Nguyen Ta
*/
#pragma once
#include "gtsam/geometry/Point2.h"
#include <iostream>
struct Feature2D
{
gtsam::Point2 m_p;
int m_idCamera; // id of the camera pose that makes this measurement
int m_idLandmark; // id of the 3D landmark that it is associated with
Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) :
m_p(p),
m_idCamera(idCamera),
m_idLandmark(idLandmark) {}
void print(const std::string& s = "") const {
std::cout << s << std::endl;
std::cout << "Pose id: " << m_idCamera << " -- Landmark id: " << m_idLandmark << std::endl;
m_p.print("\tMeasurement: ");
}
};

101
examples/vSLAMexample/README
View File

@ -1,101 +0,0 @@
README - vSLAMexample
------------------------------------------------------
vSLAMexample includes 2 simple examples using GTSAM:
- vSFMexample using visualSLAM in for structure-from-motion (SFM), and
- vISAMexample using visualSLAM and ISAM for incremental SLAM updates
The two examples use the same visual SLAM graph structure which nodes are 6d camera poses (SE3) and 3d point landmarks. Measurement factors are 2D features detected on each image, connecting its camera-pose node and the corresponding landmark nodes. There are also prior factors on each pose nodes.
Synthesized data generation
---------------------------
The data are generated by using Panda3D graphics engine to render a sequence of virtual scene with 7 colorful small 3d patches viewing by camera moving around. The patches' coordinates are given in "landmarks.txt" file. Centroids of those colorful features in the rendered images are detected and stored in "ttpy*.feat" files.
Files "ttpy*.pose" contain the poses of the virtual camera that renders the scene. A *VERY IMPORTANT* note is that the values in these "ttpy*.pose" files follow Panda3D's convention for camera frame coordinate system: "z up, y view", where as in our code, we follow OpenGL's convention: "y up, -z view". Thus, we have to change it to match with our convention. Essentially, the z- and y- axes are swapped, then the z-axis is negated to stick to the right-hand rule. Please see the function "gtsam::Pose3 readPose(const char* Fn)" in "vSLAMutils.cpp" for more information.
File "calib.txt" contains intrinsic parameters of the virtual camera. The signs are correctly adjusted to match with our projection coordinate system's convention.
Files "measurements.txt" and "poses.txt" simulate typical input data for a structure-from-motion problem. Similarly, "measurementsISAM.txt" and "posesISAM.txt" simulate the data used in SLAM context with incremental-update using ISAM.
Note that for SFM, the whole graph is solved as a whole batch problem, so the camera_id's corresponding to the feature files and pose files need to be specified in "measurements.txt" and "poses.txt", but they are not necessarily in order.
On the other hand, for ISAM, we sequentially add the camera poses and features and update after every frame; so the pose files and features files in "measurementsISAM.txt" and "posesISAM.txt" need to be specified in order (time order), even though the camera id's are not necessary.
Data file format
-----------------------------
"calib.txt":
------------
image_width image_height fx fy ox oy
"landmarks.txt"
------------
N #number of landmarks
landmark_id1 x1 y1 z1
landmark_id2 x2 y2 z2
...
landmark_idN xN yN zN
"ttpy*.feat"
------------
N #number of features
corresponding_landmark_id1 x1 y1
corresponding_landmark_id2 x2 y2
...
corresponding_landmark_idN xN yN
"ttpy*.pose"
------------
0.939693 0.34202 0 0
-0.241845 0.664463 -0.707107 0
-0.241845 0.664463 0.707107 0
34.202 -93.9693 100 1
The camera pose matrix in column order. Note that these values follows Panda3D's convention for camera coordinate frame. We have to change it to match with our convention used in the code, which follows OpenGL system. See previous section for more details.
Data For SFM:
"measurements.txt"
------------
N #number of cameras
camera_id1 featureFile1
camera_id2 featureFile2
...
camera_id3 featureFile3
"poses.txt"
------------
N #number of cameras
camera_id1 poseFile1
camera_id2 poseFile2
...
camera_id3 poseFile3
Data For ISAM:
"measurementsISAM.txt"
------------
N #number of cameras
featureFile1
featureFile2
...
featureFile3
"posesISAM.txt"
------------
N #number of cameras
poseFile1
poseFile2
...
poseFile3

147
examples/vSLAMexample/vISAMexample.cpp
View File

@ -1,147 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 vISAMexample.cpp
* @brief An ISAM example for synthesis sequence
* single camera
* @author Duy-Nguyen Ta
*/
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include "vSLAMutils.h"
#include "Feature2D.h"
using namespace std;
using namespace gtsam;
using namespace visualSLAM;
using namespace boost;
/* ************************************************************************* */
#define CALIB_FILE "calib.txt"
#define LANDMARKS_FILE "landmarks.txt"
#define POSES_FILE "poses.txt"
#define MEASUREMENTS_FILE "measurements.txt"
// Base data folder
string g_dataFolder;
// Store groundtruth values, read from files
shared_ptrK g_calib;
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
map<int, Pose3> g_poses; // map: <camera_id, pose>
std::map<int, std::vector<Feature2D> > g_measurements; // feature sets detected at each frame
// Noise models
SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
SharedNoiseModel poseSigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
/**
* Read all data: calibration file, landmarks, poses, and all features measurements
* Data is stored in global variables, which are used later to simulate incremental updates.
*/
void readAllDataISAM() {
g_calib = readCalibData(g_dataFolder + CALIB_FILE);
// Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
g_poses = readPoses(g_dataFolder, POSES_FILE);
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
}
/* ************************************************************************* */
/**
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
const Values& currentEstimate, int pose_id, const Pose3& pose,
const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements
newFactors = shared_ptr<Graph> (new Graph());
for (size_t i = 0; i < measurements.size(); i++) {
newFactors->addMeasurement(
measurements[i].m_p,
measurementSigma,
pose_id,
measurements[i].m_idLandmark,
calib);
}
// Add prior on the first pose
if(pose_id == 0)
newFactors->addPosePrior(pose_id, Pose3(), poseSigma);
// Create initial values for all new variables and add priors on new landmarks
initialValues = shared_ptr<Values> (new Values());
initialValues->insert(PoseKey(pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) {
if(!currentEstimate.exists(PointKey(measurements[i].m_idLandmark))) {
initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
}
}
}
/* ************************************************************************* */
int main(int argc, char* argv[]) {
if (argc < 2) {
cout << "Usage: vISAMexample <DataFolder>" << endl << endl;
cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
"\tlandmarks, initial poses, and feature data." << endl;
cout << "\tSample folder is in $gtsam_source_folder$/examples/Data/" << endl << endl;
cout << "Example usage: vISAMexample '$gtsam_source_folder$/examples/Data/'" << endl;
exit(0);
}
g_dataFolder = string(argv[1]) + "/";
readAllDataISAM();
// Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
int relinearizeInterval = 3;
NonlinearISAM<> isam(relinearizeInterval);
// At each frame (poseId) with new camera pose and set of associated measurements,
// create a graph of new factors and update ISAM
typedef std::map<int, std::vector<Feature2D> > FeatureMap;
Values currentEstimate = isam.estimate();
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
const int poseId = features.first;
shared_ptr<Graph> newFactors;
shared_ptr<Values> initialValues;
createNewFactors(newFactors, initialValues, currentEstimate, poseId, g_poses[poseId],
features.second, measurementSigma, g_calib);
isam.update(*newFactors, *initialValues);
currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
currentEstimate.print("Current estimate: ");
}
return 1;
}
/* ************************************************************************* */

Some files were not shown because too many files have changed in this diff Show More