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

Merge remote-tracking branch 'svn/trunk' into windows 

Conflicts:
	examples/Pose2SLAMExample.cpp
	examples/vSLAMexample/CMakeLists.txt
	examples/vSLAMexample/vISAMexample.cpp
	gtsam/nonlinear/ISAM2-impl.cpp
	gtsam/slam/pose2SLAM.cpp
	gtsam/slam/tests/testGeneralSFMFactor.cpp
	gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
	gtsam/slam/tests/testPlanarSLAM.cpp
	gtsam/slam/tests/testPose2SLAM.cpp
	tests/testDoglegOptimizer.cpp
	tests/testGraph.cpp
release/4.3a0
Richard Roberts 2012-06-04 14:13:37 +00:00
parent 318dd96dfb 872c678f02
commit 9c8377f476
153 changed files with 24831 additions and 3395 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

405
.cproject
View File

@ -433,166 +433,6 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="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="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>
<buildArguments/>
<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>
<buildArguments/>
<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>
<buildArguments/>
<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>
<buildArguments/>
<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>
<buildArguments/>
<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>
<buildArguments/>
<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>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -737,34 +577,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>
@ -897,22 +785,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 +793,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>
@ -985,14 +841,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 +865,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>
@ -1057,6 +897,59 @@
<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>
<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>
<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>
<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="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1752,18 +1645,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>
@ -1792,10 +1685,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="PlanarSLAMSelfContained_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="PlanarSLAMExample_selfcontained.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>PlanarSLAMExample_selfcontained.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1808,18 +1701,18 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExample_easy.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>Pose2SLAMExample_easy.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMwSPCG_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="Pose2SLAMwSPCG.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMwSPCG.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1832,6 +1725,30 @@
<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="LocalizationExample2.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>LocalizationExample2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExample_graph.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMExample_graph.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>

2
Doxyfile
View File

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

1
examples/CMakeLists.txt
View File

@ -30,4 +30,3 @@ foreach(example_src ${example_srcs} )
endforeach(example_src)
add_subdirectory(vSLAMexample)

111
examples/CameraResectioning.cpp
View File

@ -17,88 +17,81 @@
*/
#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 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) {
}
ADD_CLONE_NONLINEAR_FACTOR(ResectioningFactor)
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(*K_, pose);
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 = sharedSigmas(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;
}

1
examples/Data/calib.txt
View File

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

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

12
examples/LocalizationExample.cpp
View File

@ -35,7 +35,7 @@ using namespace gtsam;
*/
int main(int argc, char** argv) {
// create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
// create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
pose2SLAM::Graph graph;
// add a Gaussian prior on pose x_1
@ -55,7 +55,7 @@ int main(int argc, char** argv) {
// create (deliberatly inaccurate) initial estimate
pose2SLAM::Values initialEstimate;
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial estimate:\n ");
@ -64,11 +64,11 @@ int main(int argc, char** argv) {
result.print("\nFinal result:\n ");
// Query the marginals
Marginals marginals = graph.marginals(result);
cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(3)) << endl;
Marginals marginals = graph.marginals(result);
cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}

13
examples/LocalizationExample2.cpp
View File

@ -71,10 +71,9 @@ int main(int argc, char** argv) {
// add unary measurement factors, like GPS, on all three poses
SharedDiagonal noiseModel(Vector_(2, 0.1, 0.1)); // 10cm std on x,y
Symbol x1('x',1), x2('x',2), x3('x',3);
graph.push_back(boost::make_shared<UnaryFactor>(x1, 0, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(x2, 2, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(x3, 4, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(1, 0, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4, 0, noiseModel));
// print
graph.print("\nFactor graph:\n");
@ -94,9 +93,9 @@ int main(int argc, char** argv) {
// Query the marginals
Marginals marginals(graph, result);
cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(x1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(x2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(x3) << endl;
cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}

29
examples/PlanarSLAMExample_easy.cpp → examples/PlanarSLAMExample.cpp
View File

@ -18,6 +18,9 @@
// pull in the planar SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/planarSLAM.h>
// we will use Symbol keys
#include <gtsam/nonlinear/Symbol.h>
using namespace std;
using namespace gtsam;
@ -35,16 +38,20 @@ int main(int argc, char** argv) {
// create the graph (defined in planarSlam.h, derived from NonlinearFactorGraph)
planarSLAM::Graph graph;
// Create some keys
static Symbol i1('x',1), i2('x',2), i3('x',3);
static Symbol j1('l',1), j2('l',2);
// add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); // add directly to graph
graph.addPrior(i1, priorMean, 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)
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise);
// create a noise model for the landmark measurements
SharedDiagonal measurementNoise(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
@ -58,20 +65,20 @@ int main(int argc, char** argv) {
range32 = 2.0;
// add bearing/range factors (created by "addBearingRange")
graph.addBearingRange(1, 1, bearing11, range11, measurementNoise);
graph.addBearingRange(2, 1, bearing21, range21, measurementNoise);
graph.addBearingRange(3, 2, bearing32, range32, measurementNoise);
graph.addBearingRange(i1, j1, bearing11, range11, measurementNoise);
graph.addBearingRange(i2, j1, bearing21, range21, measurementNoise);
graph.addBearingRange(i3, j2, bearing32, range32, measurementNoise);
// print
graph.print("Factor graph");
// create (deliberatly inaccurate) initial estimate
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.insertPose(i1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(j1, Point2(1.8, 2.1));
initialEstimate.insertPoint(j2, Point2(4.1, 1.8));
initialEstimate.print("Initial estimate:\n ");

72
examples/PlanarSLAMSelfContained_advanced.cpp → examples/PlanarSLAMExample_selfcontained.cpp
View File

@ -10,34 +10,34 @@
* -------------------------------------------------------------------------- */
/**
* @file PlanarSLAMSelfContained_advanced.cpp
* @file PlanarSLAMExample_selfcontained.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>
// for all nonlinear keys
#include <gtsam/nonlinear/Symbol.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>
// for modeling measurement uncertainty - all models included here
#include <gtsam/linear/NoiseModel.h>
// for points and poses
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;
@ -52,26 +52,26 @@ using namespace gtsam;
*/
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);
Symbol i1('x',1), i2('x',2), i3('x',3);
Symbol j1('l',1), j2('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
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
PriorFactor<Pose2> posePrior(i1, priorMean, priorNoise); // 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)
SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odometry(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);
BetweenFactor<Pose2> odom12(i1, i2, odometry, odometryNoise);
BetweenFactor<Pose2> odom23(i2, i3, odometry, odometryNoise);
graph.add(odom12); // add both to graph
graph.add(odom23);
@ -88,9 +88,9 @@ int main(int argc, char** argv) {
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);
BearingRangeFactor<Pose2, Point2> meas11(i1, j1, bearing11, range11, meas_model);
BearingRangeFactor<Pose2, Point2> meas21(i2, j1, bearing21, range21, meas_model);
BearingRangeFactor<Pose2, Point2> meas32(i3, j2, bearing32, range32, meas_model);
// add the factors
graph.add(meas11);
@ -101,11 +101,11 @@ int main(int argc, char** argv) {
// 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.insert(i1, Pose2(0.5, 0.0, 0.2));
initial.insert(i2, Pose2(2.3, 0.1,-0.2));
initial.insert(i3, Pose2(4.1, 0.1, 0.1));
initial.insert(j1, Point2(1.8, 2.1));
initial.insert(j2, Point2(4.1, 1.8));
initial.print("initial estimate");
@ -126,11 +126,11 @@ int main(int argc, char** argv) {
// 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");
print(marginals.marginalCovariance(i1), "i1 covariance");
print(marginals.marginalCovariance(i2), "i2 covariance");
print(marginals.marginalCovariance(i3), "i3 covariance");
print(marginals.marginalCovariance(j1), "j1 covariance");
print(marginals.marginalCovariance(j2), "j2 covariance");
return 0;
}

34
examples/Pose2SLAMExample_easy.cpp → examples/Pose2SLAMExample.cpp
View File

@ -10,15 +10,15 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose2SLAMExample_easy.cpp
* @file Pose2SLAMExample.cpp
* @brief A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
* @date Oct 21, 2010
* @author Yong Dian Jian
*/
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <cmath>
#include <gtsam/slam/pose2SLAM.h>
#include <cmath>
using namespace std;
using namespace gtsam;
@ -26,7 +26,7 @@ using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create graph container and add factors to it
pose2SLAM::Graph graph ;
pose2SLAM::Graph graph;
// 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
@ -35,30 +35,30 @@ int main(int argc, char** argv) {
// 2b. Add odometry factors
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise);
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint
SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI/2.0), constraintUncertainty);
SharedDiagonal model(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), model);
// print
graph.print("\nFactor graph:\n");
// 3. Create the data structure to hold the initialEstimate estinmate to the solution
// 3. Create the data structure to hold the initialEstimate estimate to the solution
pose2SLAM::Values initialEstimate;
Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
Pose2 x3(4.1, 0.1, M_PI/2.0); initialEstimate.insertPose(3, x3);
Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4);
Pose2 x5(2.1, 2.1,-M_PI/2.0); initialEstimate.insertPose(5, x5);
initialEstimate.print("\nInitial estimate:\n ");
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, M_PI_2));
initialEstimate.insertPose(4, Pose2(4.0, 2.0, M_PI));
initialEstimate.insertPose(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial estimate:\n");
// 4. Single Step Optimization using Levenberg-Marquardt
pose2SLAM::Values result = graph.optimize(initialEstimate);
result.print("\nFinal result:\n ");
result.print("\nFinal result:\n");
return 0;
}

43
examples/Pose2SLAMExample_advanced.cpp
View File

@ -16,48 +16,41 @@
* @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>
#include <boost/shared_ptr.hpp>
#include <cmath>
#include <iostream>
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;
pose2SLAM::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
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); // 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);
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */
/* 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(2, Pose2(2.3, 0.1, -0.2));
initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
initial.print("initial estimate");
@ -70,15 +63,15 @@ int main(int argc, char** argv) {
params.absoluteErrorTol = 1e-15;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
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));
Matrix covariance1 = marginals.marginalCovariance(1);
Matrix covariance2 = marginals.marginalCovariance(2);
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");

53
examples/Pose2SLAMExample_graph.cpp Normal file
View File

@ -0,0 +1,53 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Pose2SLAMExample_graph->cpp
* @brief Read graph from file and perform GraphSLAM
* @date June 3, 2012
* @author Frank Dellaert
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/Marginals.h>
#include <boost/tuple/tuple.hpp>
#include <cmath>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Read File and create graph and initial estimate
// we are in build/examples, data is in examples/Data
pose2SLAM::Graph::shared_ptr graph ;
pose2SLAM::Values::shared_ptr initial;
SharedDiagonal model(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0));
boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model);
initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.01, 0.01, 0.01));
graph->addPrior(0, priorMean, priorNoise);
// Single Step Optimization using Levenberg-Marquardt
pose2SLAM::Values result = graph->optimize(*initial);
result.print("\nFinal result:\n");
// Plot the covariance of the last pose
Marginals marginals(*graph, result);
cout.precision(2);
cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl;
return 0;
}

77
examples/Pose2SLAMwSPCG.cpp Normal file
View File

@ -0,0 +1,77 @@
/* ----------------------------------------------------------------------------
* 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
*/
#include <gtsam/linear/IterativeOptimizationParameters.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/pose2SLAM.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main(void) {
// 1. Create graph container and add factors to it
pose2SLAM::Graph graph ;
// 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise);
// 2b. Add odometry factors
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint
SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
// print
graph.print("\nFactor graph:\n");
// 3. Create the data structure to hold the initialEstimate estinmate to the solution
pose2SLAM::Values initialEstimate;
Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3);
Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4);
Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5);
initialEstimate.print("\nInitial estimate:\n ");
cout << "initial error = " << graph.error(initialEstimate) << endl ;
// 4. Single Step Optimization using Levenberg-Marquardt
// Note: Although there are many options in IterativeOptimizationParameters,
// the SimpleSPCGSolver doesn't actually use all of them at this moment.
// More detail in the next release.
LevenbergMarquardtParams param;
param.linearSolverType = SuccessiveLinearizationParams::CG;
param.iterativeParams = boost::make_shared<IterativeOptimizationParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param);
Values result = optimizer.optimize();
cout << "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

91
examples/VisualSLAMExampleData.h Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* 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 VisualSLAMSimulatedData.cpp
* @brief Generate ground-truth simulated data for VisualSLAM examples (SFM and ISAM2)
* @author Duy-Nguyen Ta
*/
#pragma once
#include <vector>
#include <map>
#include <gtsam/geometry/SimpleCamera.h>
/* ************************************************************************* */
/**
* Simulated data for the example:
* - 8 Landmarks: (10,10,10) (-10,10,10) (-10,-10,10) (10,-10,10)
* (10,10,-10) (-10,10,-10) (-10,-10,-10) (10,-10,-10)
* - n 90-deg-FoV cameras with the same calibration parameters:
* f = 50.0, Image: 100x100, center: 50.0, 50.0
* and ground-truth poses on a circle around the landmarks looking at the world's origin:
* Rot3(-sin(theta), 0, -cos(theta),
* cos(theta), 0, -sin(theta),
* 0, -1, 0 ),
* Point3(r*cos(theta), r*sin(theta), 0.0)
* (theta += 2*pi/N)
* - Measurement noise: 1 pix sigma
*/
struct VisualSLAMExampleData {
gtsam::shared_ptrK sK; // camera calibration parameters
std::vector<gtsam::Pose3> poses; // ground-truth camera poses
gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM)
std::vector<gtsam::Point3> landmarks; // ground-truth landmarks
std::map<int, vector<gtsam::Point2> > z; // 2D measurements of landmarks in each camera frame
gtsam::SharedDiagonal noiseZ; // measurement noise (noiseModel::Isotropic::Sigma(2, 5.0f));
gtsam::SharedDiagonal noiseX; // noise for camera poses
gtsam::SharedDiagonal noiseL; // noise for landmarks
static const VisualSLAMExampleData generate() {
VisualSLAMExampleData data;
// Landmarks (ground truth)
data.landmarks.push_back(gtsam::Point3(10.0,10.0,10.0));
data.landmarks.push_back(gtsam::Point3(-10.0,10.0,10.0));
data.landmarks.push_back(gtsam::Point3(-10.0,-10.0,10.0));
data.landmarks.push_back(gtsam::Point3(10.0,-10.0,10.0));
data.landmarks.push_back(gtsam::Point3(10.0,10.0,-10.0));
data.landmarks.push_back(gtsam::Point3(-10.0,10.0,-10.0));
data.landmarks.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
data.landmarks.push_back(gtsam::Point3(10.0,-10.0,-10.0));
// Camera calibration parameters
data.sK = gtsam::shared_ptrK(new gtsam::Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// n camera poses
int n = 8;
double theta = 0.0;
double r = 30.0;
for (int i=0; i<n; ++i) {
theta += 2*M_PI/n;
data.poses.push_back(gtsam::Pose3(
gtsam::Rot3(-sin(theta), 0.0, -cos(theta),
cos(theta), 0.0, -sin(theta),
0.0, -1.0, 0.0),
gtsam::Point3(r*cos(theta), r*sin(theta), 0.0)));
}
data.odometry = data.poses[0].between(data.poses[1]);
// Simulated measurements with Gaussian noise
data.noiseZ = gtsam::sharedSigma(2, 1.0);
for (size_t i=0; i<data.poses.size(); ++i) {
for (size_t j=0; j<data.landmarks.size(); ++j) {
gtsam::SimpleCamera camera(data.poses[i], *data.sK);
data.z[i].push_back(camera.project(data.landmarks[j]) + gtsam::Point2(data.noiseZ->sample()));
}
}
data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1));
data.noiseL = gtsam::sharedSigma(3, 0.1);
return data;
}
};

64
examples/VisualSLAMforSFMExample.cpp Normal file
View File

@ -0,0 +1,64 @@
/* ----------------------------------------------------------------------------
* 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 VisualSLAMforSFMExample.cpp
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
* @author Duy-Nguyen Ta
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include "VisualSLAMExampleData.h"
using namespace std;
using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
VisualSLAMExampleData data = VisualSLAMExampleData::generate();
/* 1. Create graph *///using the 2D measurements (features) and the calibration data
visualSLAM::Graph graph;
/* 2. Add factors to the graph */
// 2a. Measurement factors
for (size_t i=0; i<data.poses.size(); ++i) {
for (size_t j=0; j<data.landmarks.size(); ++j)
graph.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
}
// 2b. Prior factor for the first pose to resolve ambiguity (not needed for LevenbergMarquardtOptimizer)
graph.addPosePrior(X(0), data.poses[0], data.noiseX);
/* 3. Initial estimates for variable nodes, simulated by Gaussian noises */
Values initial;
for (size_t i=0; i<data.poses.size(); ++i)
initial.insert(X(i), data.poses[i]*Pose3::Expmap(data.noiseX->sample()));
for (size_t j=0; j<data.landmarks.size(); ++j)
initial.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
initial.print("Intial Estimates: ");
/* 4. Optimize the graph and print results */
visualSLAM::Values result = GaussNewtonOptimizer(graph, initial).optimize();
// visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("Final results: ");
return 0;
}
/* ************************************************************************* */

113
examples/VisualSLAMwISAM2Example.cpp Normal file
View File

@ -0,0 +1,113 @@
/* ----------------------------------------------------------------------------
* 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 VisualSLAMwISAM2Example.cpp
* @brief An ISAM example for synthesis sequence, single camera
* @author Duy-Nguyen Ta
*/
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h>
#include "VisualSLAMExampleData.h"
using namespace std;
using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
VisualSLAMExampleData data = VisualSLAMExampleData::generate();
/* 1. Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates */
int relinearizeInterval = 3;
NonlinearISAM<> isam(relinearizeInterval);
/* 2. At each frame (poseId) with new camera pose and set of associated measurements,
* create a graph of new factors and update ISAM */
// Store the current best estimate from ISAM
Values currentEstimate;
// First two frames:
// Add factors and initial values for the first two poses and landmarks then update ISAM.
// Note: measurements from the first pose only are not enough to update ISAM:
// the system is underconstrained.
{
visualSLAM::Graph newFactors;
// First pose with prior factor
newFactors.addPosePrior(X(0), data.poses[0], data.noiseX);
// Second pose with odometry measurement, simulated by adding Gaussian noise to the ground-truth.
Pose3 odoMeasurement = data.odometry*Pose3::Expmap(data.noiseX->sample());
newFactors.push_back( boost::shared_ptr<BetweenFactor<Pose3> >(
new BetweenFactor<Pose3>(X(0), X(1), odoMeasurement, data.noiseX)));
// Visual measurements at both poses
for (size_t i=0; i<2; ++i) {
for (size_t j=0; j<data.z[i].size(); ++j) {
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
}
}
// Initial values for the first two poses, simulated with Gaussian noise
Values initials;
Pose3 pose0Init = data.poses[0]*Pose3::Expmap(data.noiseX->sample());
initials.insert(X(0), pose0Init);
initials.insert(X(1), pose0Init*odoMeasurement);
// Initial values for the landmarks, simulated with Gaussian noise
for (size_t j=0; j<data.landmarks.size(); ++j)
initials.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
// Update ISAM the first time and obtain the current estimate
isam.update(newFactors, initials);
currentEstimate = isam.estimate();
cout << "Frame 0 and 1: " << endl;
currentEstimate.print("Current estimate: ");
}
// Subsequent frames: Add new odometry and measurement factors and initial values,
// then update ISAM at each frame
for (size_t i=2; i<data.poses.size(); ++i) {
visualSLAM::Graph newFactors;
// Factor for odometry measurements, simulated by adding Gaussian noise to the ground-truth.
Pose3 odoMeasurement = data.odometry*Pose3::Expmap(data.noiseX->sample());
newFactors.push_back( boost::shared_ptr<BetweenFactor<Pose3> >(
new BetweenFactor<Pose3>(X(i-1), X(i), odoMeasurement, data.noiseX)));
// Factors for visual measurements
for (size_t j=0; j<data.z[i].size(); ++j) {
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
}
// Initial estimates for the new node Xi, simulated by Gaussian noises
Values initials;
initials.insert(X(i), currentEstimate.at<Pose3>(X(i-1))*odoMeasurement);
// update ISAM
isam.update(newFactors, initials);
currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
cout << "Frame " << i << ": " << endl;
currentEstimate.print("Current estimate: ");
}
return 0;
}
/* ************************************************************************* */

1
examples/easyPoint2KalmanFilter.cpp
View File

@ -22,6 +22,7 @@
*/
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Point2.h>

6
examples/matlab/LocalizationExample.m
View File

@ -45,9 +45,9 @@ result.print(sprintf('\nFinal result:\n '));
%% Query the marginals
marginals = graph.marginals(result);
x{1}=gtsamSymbol('x',1); P{1}=marginals.marginalCovariance(x{1}.key)
x{2}=gtsamSymbol('x',2); P{2}=marginals.marginalCovariance(x{2}.key)
x{3}=gtsamSymbol('x',3); P{3}=marginals.marginalCovariance(x{3}.key)
P{1}=marginals.marginalCovariance(1);
P{2}=marginals.marginalCovariance(2);
P{3}=marginals.marginalCovariance(3);
%% Plot Trajectory
figure(1)

18
examples/matlab/PlanarSLAMExample_easy.m → examples/matlab/PlanarSLAMExample.m
View File

@ -20,24 +20,24 @@
% - Landmarks are 2 meters away from the robot trajectory
%% Create keys for variables
x1 = 1; x2 = 2; x3 = 3;
l1 = 1; l2 = 2;
x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3);
l1 = symbol('l',1); l2 = symbol('l',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
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, priorMean, priorNoise); % 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);
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odometry, odometryNoise);
graph.addOdometry(x2, x3, odometry, odometryNoise);
%% Add measurements
% general noisemodel for measurements

43
examples/matlab/Pose2SLAMExample_easy.m → examples/matlab/Pose2SLAMExample.m
View File

@ -1,9 +1,9 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% 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
@ -18,40 +18,39 @@
% - 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
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(1, priorMean, priorNoise); % 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);
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add measurements
% general noisemodel for measurements
meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
%% Add pose constraint
model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
% print
graph.print('full graph');
graph.print(sprintf('\nFactor graph:\n'));
%% 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');
initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 ));
initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 ));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2));
initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi ));
initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
initialEstimate.print(sprintf('\nInitial estimate:\n'));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result.print('final result');
result.print(sprintf('\nFinal result:\n'));

28
examples/matlab/Pose2SLAMExample_advanced.m
View File

@ -20,24 +20,21 @@
% - 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
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(1, priorMean, priorNoise); % 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);
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
%% Add measurements
% general noisemodel for measurements
@ -48,9 +45,9 @@ 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.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print('initial estimate');
@ -62,10 +59,7 @@ initialEstimate.print('initial estimate');
%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params);
%result.print('final result');
% %
% % disp('\\\');
% %
% % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result.print('final result');

48
examples/matlab/Pose2SLAMExample_circle.m Normal file
View File

@ -0,0 +1,48 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
hexagon = pose2SLAMValues_Circle(6,1.0);
p0 = hexagon.pose(0);
p1 = hexagon.pose(1);
%% create a Pose graph with one equality constraint and one measurement
fg = pose2SLAMGraph;
fg.addPoseConstraint(0, p0);
delta = p0.between(p1);
covariance = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]);
fg.addOdometry(0,1, delta, covariance);
fg.addOdometry(1,2, delta, covariance);
fg.addOdometry(2,3, delta, covariance);
fg.addOdometry(3,4, delta, covariance);
fg.addOdometry(4,5, delta, covariance);
fg.addOdometry(5,0, delta, covariance);
%% Create initial config
initial = pose2SLAMValues;
initial.insertPose(0, p0);
initial.insertPose(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]'));
initial.insertPose(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]'));
initial.insertPose(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]'));
initial.insertPose(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]'));
initial.insertPose(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]'));
%% Plot Initial Estimate
figure(1);clf
plot(initial.xs(),initial.ys(),'g-*'); axis equal
%% optimize
result = fg.optimize(initial);
%% Show Result
hold on; plot(result.xs(),result.ys(),'b-*')
result.print(sprintf('\nFinal result:\n'));

39
examples/matlab/Pose2SLAMExample_graph.m Normal file
View File

@ -0,0 +1,39 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Initialize graph, initial estimate, and odometry noise
model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]);
[graph,initial]=load2D('../Data/w100-odom.graph',model);
initial.print(sprintf('Initial estimate:\n'));
%% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = gtsamSharedNoiseModel_Sigmas([0.01; 0.01; 0.01]);
graph.addPrior(0, priorMean, priorNoise); % add directly to graph
%% Plot Initial Estimate
figure(1);clf
plot(initial.xs(),initial.ys(),'g-*'); axis equal
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initial);
hold on; plot(result.xs(),result.ys(),'b-*')
result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses
marginals = graph.marginals(result);
for i=1:result.size()-1
pose_i = result.pose(i);
P{i}=marginals.marginalCovariance(i);
covarianceEllipse([pose_i.x;pose_i.y],P{i},'b')
end
fprintf(1,'%.5f %.5f %.5f\n',P{99})

49
examples/matlab/Pose3SLAMExample_circle.m Normal file
View File

@ -0,0 +1,49 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
hexagon = pose3SLAMValues_Circle(6,1.0);
p0 = hexagon.pose(0);
p1 = hexagon.pose(1);
%% create a Pose graph with one equality constraint and one measurement
fg = pose3SLAMGraph;
fg.addHardConstraint(0, p0);
delta = p0.between(p1);
covariance = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
fg.addConstraint(0,1, delta, covariance);
fg.addConstraint(1,2, delta, covariance);
fg.addConstraint(2,3, delta, covariance);
fg.addConstraint(3,4, delta, covariance);
fg.addConstraint(4,5, delta, covariance);
fg.addConstraint(5,0, delta, covariance);
%% Create initial config
initial = pose3SLAMValues;
s = 0.10;
initial.insertPose(0, p0);
initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1)));
initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1)));
initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1)));
initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1)));
initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
%% Plot Initial Estimate
figure(1);clf
plot3(initial.xs(),initial.ys(),initial.zs(),'g-*'); axis equal
%% optimize
result = fg.optimize(initial);
%% Show Result
hold on; plot3(result.xs(),result.ys(),result.zs(),'b-*')
result.print(sprintf('\nFinal result:\n'));

33
examples/matlab/Pose3SLAMExample_graph.m Normal file
View File

@ -0,0 +1,33 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = 2500;
% filename = '../Data/sphere_smallnoise.graph';
% filename = '../Data/sphere2500_groundtruth.txt';
filename = '../Data/sphere2500.txt';
%% Initialize graph, initial estimate, and odometry noise
model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
[graph,initial]=load3D(filename,model,true,N);
first = initial.pose(0);
graph.addHardConstraint(0, first);
%% Plot Initial Estimate
figure(1);clf
plot3(first.x(),first.y(),first.z(),'r*'); hold on
plot3DTrajectory(initial,'g-',false);
%% Read again, now all constraints
[graph,discard]=load3D(filename,model,false,N);
graph.addHardConstraint(0, first);
result = graph.optimize(initial); % start from old result
plot3DTrajectory(result,'r-',false); axis equal;

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

@ -0,0 +1,29 @@
function [graph,initial] = load2D(filename,model)
% load2D: read TORO pose graph
% cannot read noise model from file yet, uses specified model
fid = fopen(filename);
if fid < 0
error(['load2D: Cannot open file ' filename]);
end
% scan all lines into a cell array
columns=textscan(fid,'%s','delimiter','\n');
fclose(fid);
lines=columns{1};
% loop over lines and add vertices
graph = pose2SLAMGraph;
initial = pose2SLAMValues;
n=size(lines,1);
for i=1:n
line_i=lines{i};
if strcmp('VERTEX2',line_i(1:7))
v = textscan(line_i,'%s %d %f %f %f',1);
initial.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5}));
elseif strcmp('EDGE2',line_i(1:5))
e = textscan(line_i,'%s %d %d %f %f %f',1);
graph.addOdometry(e{2}, e{3}, gtsamPose2(e{4}, e{5}, e{6}), model);
end
end

56
examples/matlab/load3D.m Normal file
View File

@ -0,0 +1,56 @@
function [graph,initial] = load3D(filename,model,successive,N)
% load3D: read TORO 3D pose graph
% cannot read noise model from file yet, uses specified model
% if [successive] is tru, constructs initial estimate from odometry
if nargin<3, successive=false; end
fid = fopen(filename);
if fid < 0
error(['load2D: Cannot open file ' filename]);
end
% scan all lines into a cell array
columns=textscan(fid,'%s','delimiter','\n');
fclose(fid);
lines=columns{1};
% loop over lines and add vertices
graph = pose3SLAMGraph;
initial = pose3SLAMValues;
origin=gtsamPose3;
initial.insertPose(0,origin);
n=size(lines,1);
if nargin<4, N=n;end
for i=1:n
line_i=lines{i};
if strcmp('VERTEX3',line_i(1:7))
v = textscan(line_i,'%s %d %f %f %f %f %f %f',1);
i=v{2};
if (~successive & i<N | successive & i==0)
t = gtsamPoint3(v{3}, v{4}, v{5});
R = gtsamRot3_ypr(v{8}, -v{7}, v{6});
initial.insertPose(i, gtsamPose3(R,t));
end
elseif strcmp('EDGE3',line_i(1:5))
e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1);
i1=e{2};
i2=e{3};
if i1<N & i2<N
if ~successive | abs(i2-i1)==1
t = gtsamPoint3(e{4}, e{5}, e{6});
R = gtsamRot3_ypr(e{9}, e{8}, e{7});
dpose = gtsamPose3(R,t);
graph.addConstraint(i1, i2, dpose, model);
if successive
if i2>i1
initial.insertPose(i2,initial.pose(i1).compose(dpose));
else
initial.insertPose(i1,initial.pose(i2).compose(dpose.inverse));
end
end
end
end
end
end

15
examples/matlab/plot3DTrajectory.m Normal file
View File

@ -0,0 +1,15 @@
function plot3DTrajectory(values,style,frames)
% plot3DTrajectory
if nargin<3,frames=false;end
plot3(values.xs(),values.ys(),values.zs(),style); hold on
if frames
for i=0:N-1
pose = values.pose(i);
t = pose.translation;
R = pose.rotation.matrix;
quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),'r');
quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),'g');
quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),'b');
end
end

4
examples/matlab/symbol.m Normal file
View File

@ -0,0 +1,4 @@
function key = symbol(c,i)
% generate a key corresponding to a symbol
s = gtsamSymbol(c,i);
key = s.key();

22
examples/vSLAMexample/CMakeLists.txt
View File

@ -1,22 +0,0 @@
# Build vSLAMexample
message(STATUS "Adding Example vISAMexample")
add_executable(vISAMexample vISAMexample.cpp vSLAMutils.cpp)
target_link_libraries(vISAMexample gtsam-static)
if(NOT MSVC)
add_dependencies(examples vISAMexample)
else()
set_property(TARGET vISAMexample PROPERTY FOLDER "Examples")
endif()
message(STATUS "Adding Example vSFMexample")
add_executable(vSFMexample vSFMexample.cpp vSLAMutils.cpp)
target_link_libraries(vSFMexample gtsam-static)
if(NOT MSVC)
add_dependencies(examples vSFMexample)
else()
set_property(TARGET vSFMexample PROPERTY FOLDER "Examples")
endif()

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

144
examples/vSLAMexample/vISAMexample.cpp
View File

@ -1,144 +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(boost::shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
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 = boost::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);
}
// ... we need priors on the new pose and all new landmarks
newFactors->addPosePrior(pose_id, pose, poseSigma);
for (size_t i = 0; i < measurements.size(); i++) {
newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
}
// Create initial values for all nodes in the newFactors
initialValues = boost::shared_ptr<Values> (new Values());
initialValues->insert(PoseKey(pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(PointKey(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;
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
const int poseId = features.first;
boost::shared_ptr<Graph> newFactors;
boost::shared_ptr<Values> initialValues;
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
features.second, measurementSigma, g_calib);
isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
currentEstimate.print("Current estimate: ");
}
return 1;
}
/* ************************************************************************* */

157
examples/vSLAMexample/vSFMexample.cpp
View File

@ -1,157 +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 vSFMexample.cpp
* @brief An vSFM example for synthesis sequence
* single camera
* @author Duy-Nguyen Ta
*/
#include <boost/shared_ptr.hpp>
using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/PriorFactor.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::vector<Feature2D> g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position}
// Noise models
SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
/* ************************************************************************* */
/**
* Read all data: calibration file, landmarks, poses, and all features measurements
* Data is stored in global variables.
*/
void readAllData() {
g_calib = readCalibData(g_dataFolder + "/" + CALIB_FILE);
// Read groundtruth landmarks' positions. These will be used later as intial estimates 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 = readAllMeasurements(g_dataFolder, MEASUREMENTS_FILE);
}
/* ************************************************************************* */
/**
* Setup vSLAM graph
* by adding and linking 2D features (measurements) detected in each captured image
* with their corresponding landmarks.
*/
Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
Graph g;
cout << "Built graph: " << endl;
for (size_t i = 0; i < measurements.size(); i++) {
measurements[i].print();
g.addMeasurement(
measurements[i].m_p,
measurementSigma,
measurements[i].m_idCamera,
measurements[i].m_idLandmark,
calib);
}
return g;
}
/* ************************************************************************* */
/**
* Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
* The returned Values structure contains all initial values for all nodes.
*/
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
Values initValues;
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert(PointKey(lmit->first), lmit->second);
// Initialize camera poses.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert(PoseKey(poseit->first), poseit->second);
return initValues;
}
/* ************************************************************************* */
int main(int argc, char* argv[]) {
if (argc < 2) {
cout << "Usage: vSFMexample <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: vSFMexample '$gtsam_source_folder$/examples/Data'" << endl;
exit(0);
}
g_dataFolder = string(argv[1]) + "/";
readAllData();
// Create a graph using the 2D measurements (features) and the calibration data
Graph graph(setupGraph(g_measurements, measurementSigma, g_calib));
// Create an initial Values structure using groundtruth values as the initial estimates
Values initialEstimates(initialize(g_landmarks, g_poses));
cout << "*******************************************************" << endl;
initialEstimates.print("INITIAL ESTIMATES: ");
// Add prior factor for all poses in the graph
map<int, Pose3>::iterator poseit = g_poses.begin();
for (; poseit != g_poses.end(); poseit++)
graph.addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
// Optimize the graph
cout << "*******************************************************" << endl;
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::DAMPED;
visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimize();
// Print final results
cout << "*******************************************************" << endl;
result.print("FINAL RESULTS: ");
return 0;
}
/* ************************************************************************* */

176
examples/vSLAMexample/vSLAMutils.cpp
View File

@ -1,176 +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 VSLAMutils.cpp
* @brief
* @author Duy-Nguyen Ta
*/
#include "vSLAMutils.h"
#include <fstream>
#include <cstdio>
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
std::map<int, Point3> readLandMarks(const std::string& landmarkFile) {
ifstream file(landmarkFile.c_str());
if (!file) {
cout << "Cannot read landmark file: " << landmarkFile << endl;
exit(0);
}
int num;
file >> num;
std::map<int, Point3> landmarks;
landmarks.clear();
for (int i = 0; i<num; i++) {
int color_id;
float x, y, z;
file >> color_id >> x >> y >> z;
landmarks[color_id] = Point3(x, y, z);
}
file.close();
return landmarks;
}
/* ************************************************************************* */
gtsam::Pose3 readPose(const char* Fn) {
ifstream poseFile(Fn);
if (!poseFile) {
cout << "Cannot read pose file: " << Fn << endl;
exit(0);
}
double v[16];
for (int i = 0; i<16; i++)
poseFile >> v[i];
poseFile.close();
Matrix T = Matrix_(4,4, v); // row order !!!
Pose3 pose(T);
return pose;
}
/* ************************************************************************* */
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn) {
ifstream posesFile((baseFolder+posesFn).c_str());
if (!posesFile) {
cout << "Cannot read all pose file: " << posesFn << endl;
exit(0);
}
int numPoses;
posesFile >> numPoses;
map<int, Pose3> poses;
for (int i = 0; i<numPoses; i++) {
int poseId;
posesFile >> poseId;
string poseFileName;
posesFile >> poseFileName;
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
poses[poseId] = pose;
}
return poses;
}
/* ************************************************************************* */
gtsam::shared_ptrK readCalibData(const std::string& calibFn) {
ifstream calibFile(calibFn.c_str());
if (!calibFile) {
cout << "Cannot read calib file: " << calibFn << endl;
exit(0);
}
int imX, imY;
float fx, fy, ox, oy;
calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
calibFile.close();
return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0
}
/* ************************************************************************* */
std::vector<Feature2D> readFeatures(int pose_id, const char* filename) {
ifstream file(filename);
if (!file) {
cout << "Cannot read feature file: " << filename<< endl;
exit(0);
}
int numFeatures;
file >> numFeatures ;
std::vector<Feature2D> vFeatures_;
for (int i = 0; i < numFeatures; i++) {
int landmark_id; double x, y;
file >> landmark_id >> x >> y;
vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
}
file.close();
return vFeatures_;
}
/* ************************************************************************* */
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn) {
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
if (!measurementsFile) {
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
exit(0);
}
int numPoses;
measurementsFile >> numPoses;
vector<Feature2D> allFeatures;
allFeatures.clear();
for (int i = 0; i<numPoses; i++) {
int poseId;
measurementsFile >> poseId;
string featureFileName;
measurementsFile >> featureFileName;
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
allFeatures.insert( allFeatures.end(), features.begin(), features.end() );
}
return allFeatures;
}
/* ************************************************************************* */
std::map<int, std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
if (!measurementsFile) {
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
exit(0);
}
int numPoses;
measurementsFile >> numPoses;
std::map<int, std::vector<Feature2D> > allFeatures;
for (int i = 0; i<numPoses; i++) {
int poseId;
measurementsFile >> poseId;
string featureFileName;
measurementsFile >> featureFileName;
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
allFeatures[poseId] = features;
}
return allFeatures;
}

37
examples/vSLAMexample/vSLAMutils.h
View File

@ -1,37 +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.cpp
* @brief
* @author Duy-Nguyen Ta
*/
#pragma once
#include <map>
#include <vector>
#include "Feature2D.h"
#include "gtsam/geometry/Pose3.h"
#include "gtsam/geometry/Point3.h"
#include "gtsam/geometry/Cal3_S2.h"
std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile);
gtsam::Pose3 readPose(const char* poseFn);
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFN);
gtsam::shared_ptrK readCalibData(const std::string& calibFn);
std::vector<Feature2D> readFeatureFile(const char* filename);
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
std::map<int, std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);

497
gtsam.h
View File

@ -9,7 +9,7 @@
* Only one Method/Constructor per line
* Methods can return
* - Eigen types: Matrix, Vector
* - C/C++ basic types: string, bool, size_t, int, double, char
* - C/C++ basic types: string, bool, size_t, size_t, double, char
* - void
* - Any class with which be copied with boost::make_shared()
* - boost::shared_ptr of any object type
@ -19,7 +19,7 @@
* Arguments to functions any of
* - Eigen types: Matrix, Vector
* - Eigen types and classes as an optionally const reference
* - C/C++ basic types: string, bool, size_t, int, double
* - C/C++ basic types: string, bool, size_t, size_t, double, char
* - Any class with which be copied with boost::make_shared() (except Eigen)
* - boost::shared_ptr of any object type (except Eigen)
* Comments can use either C++ or C style, with multiple lines
@ -68,17 +68,29 @@ namespace gtsam {
//*************************************************************************
class Point2 {
Point2();
Point2(double x, double y);
static gtsam::Point2 Expmap(Vector v);
Point2();
Point2(double x, double y);
static gtsam::Point2 Expmap(Vector v);
static Vector Logmap(const gtsam::Point2& p);
void print(string s) const;
double x();
double y();
Vector localCoordinates(const gtsam::Point2& p);
Vector localCoordinates(const gtsam::Point2& p);
gtsam::Point2 compose(const gtsam::Point2& p2);
gtsam::Point2 between(const gtsam::Point2& p2);
gtsam::Point2 retract(Vector v);
gtsam::Point2 retract(Vector v);
};
class StereoPoint2 {
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
static gtsam::StereoPoint2 Expmap(Vector v);
static Vector Logmap(const gtsam::StereoPoint2& p);
void print(string s) const;
Vector localCoordinates(const gtsam::StereoPoint2& p);
gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2);
gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2);
gtsam::StereoPoint2 retract(Vector v);
};
class Point3 {
@ -124,22 +136,22 @@ class Rot2 {
class Rot3 {
Rot3();
Rot3(Matrix R);
static gtsam::Rot3 Rx(double t);
static gtsam::Rot3 Ry(double t);
static gtsam::Rot3 Rz(double t);
static gtsam::Rot3 Rx(double t);
static gtsam::Rot3 Ry(double t);
static gtsam::Rot3 Rz(double t);
// static gtsam::Rot3 RzRyRx(double x, double y, double z); // FIXME: overloaded functions don't work yet
static gtsam::Rot3 RzRyRx(Vector xyz);
static gtsam::Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading)
static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
static gtsam::Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft)
static gtsam::Rot3 ypr(double y, double p, double r);
static gtsam::Rot3 quaternion(double w, double x, double y, double z);
static gtsam::Rot3 rodriguez(Vector v);
static gtsam::Rot3 RzRyRx(Vector xyz);
static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading)
static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft)
static gtsam::Rot3 ypr(double y, double p, double r);
static gtsam::Rot3 quaternion(double w, double x, double y, double z);
static gtsam::Rot3 rodriguez(Vector v);
void print(string s) const;
bool equals(const gtsam::Rot3& rot, double tol) const;
static gtsam::Rot3 identity();
gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
gtsam::Rot3 inverse() const;
static gtsam::Rot3 identity();
gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
gtsam::Rot3 inverse() const;
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
gtsam::Point3 rotate(const gtsam::Point3& p) const;
gtsam::Point3 unrotate(const gtsam::Point3& p) const;
@ -150,13 +162,13 @@ class Rot3 {
static Vector Logmap(const gtsam::Rot3& p);
Matrix matrix() const;
Matrix transpose() const;
gtsam::Point3 column(int index) const;
gtsam::Point3 column(size_t index) const;
Vector xyz() const;
Vector ypr() const;
Vector rpy() const;
double roll() const;
double pitch() const;
double yaw() const;
double roll() const;
double pitch() const;
double yaw() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
};
@ -173,7 +185,7 @@ class Pose2 {
double x() const;
double y() const;
double theta() const;
int dim() const;
size_t dim() const;
Vector localCoordinates(const gtsam::Pose2& p);
gtsam::Pose2 retract(Vector v);
gtsam::Pose2 compose(const gtsam::Pose2& p2);
@ -185,47 +197,86 @@ class Pose2 {
};
class Pose3 {
// Standard Constructors
Pose3();
Pose3(const gtsam::Pose3& pose);
Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
Pose3(Matrix t);
Pose3(const gtsam::Pose2& pose2);
static gtsam::Pose3 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose3& p);
Pose3(Matrix t);
// Testable
void print(string s) const;
bool equals(const gtsam::Pose3& pose, double tol) const;
// Group
static gtsam::Pose3 identity();
gtsam::Pose3 inverse();
gtsam::Pose3 compose(const gtsam::Pose3& p2);
gtsam::Pose3 between(const gtsam::Pose3& p2);
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Pose3 retract(Vector v);
gtsam::Pose3 retractFirstOrder(Vector v);
Vector localCoordinates(const gtsam::Pose3& T2) const;
// Lie Group
static gtsam::Pose3 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose3& p);
Matrix adjointMap() const;
Vector adjoint(const Vector& xi) const;
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
// Group Action on Point3
gtsam::Point3 transform_from(const gtsam::Point3& p) const;
gtsam::Point3 transform_to(const gtsam::Point3& p) const;
// Standard Interface
gtsam::Rot3 rotation() const;
gtsam::Point3 translation() const;
double x() const;
double y() const;
double z() const;
Matrix matrix() const;
Matrix adjointMap() const;
gtsam::Pose3 compose(const gtsam::Pose3& p2);
gtsam::Pose3 between(const gtsam::Pose3& p2);
gtsam::Pose3 retract(Vector v);
gtsam::Pose3 retractFirstOrder(Vector v);
Vector localCoordinates(const gtsam::Pose3& T2) const;
gtsam::Point3 translation() const;
gtsam::Rot3 rotation() const;
gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const;
double range(const gtsam::Point3& point);
// double range(const gtsam::Pose3& pose);
};
class Cal3_S2 {
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
void print(string s) const;
};
class Cal3_S2Stereo {
Cal3_S2Stereo();
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
void print(string s) const;
};
class CalibratedCamera {
CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose);
CalibratedCamera(const Vector& v);
void print(string s) const;
bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose);
CalibratedCamera(const Vector& v);
gtsam::Pose3 pose() const;
void print(string s) const;
bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
gtsam::CalibratedCamera inverse() const;
gtsam::CalibratedCamera level(const gtsam::Pose2& pose2, double height);
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
gtsam::Pose3 pose() const;
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
gtsam::CalibratedCamera inverse() const;
gtsam::CalibratedCamera level(const gtsam::Pose2& pose2, double height);
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
};
@ -263,25 +314,25 @@ class SharedNoiseModel {
class VectorValues {
VectorValues();
VectorValues(int nVars, int varDim);
VectorValues(size_t nVars, size_t varDim);
void print(string s) const;
bool equals(const gtsam::VectorValues& expected, double tol) const;
int size() const;
void insert(int j, Vector value);
size_t size() const;
void insert(size_t j, Vector value);
};
class GaussianConditional {
GaussianConditional(int key, Vector d, Matrix R, Vector sigmas);
GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
Vector sigmas);
GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
int name2, Matrix T, Vector sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T, Vector sigmas);
void print(string s) const;
bool equals(const gtsam::GaussianConditional &cg, double tol) const;
};
class GaussianDensity {
GaussianDensity(int key, Vector d, Matrix R, Vector sigmas);
GaussianDensity(size_t key, Vector d, Matrix R, Vector sigmas);
void print(string s) const;
Vector mean() const;
Matrix information() const;
@ -305,10 +356,11 @@ class GaussianFactor {
class JacobianFactor {
JacobianFactor();
JacobianFactor(Vector b_in);
JacobianFactor(int i1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, Vector b,
JacobianFactor(size_t i1, Matrix A1, Vector b,
const gtsam::SharedDiagonal& model);
JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, int i3, Matrix A3,
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
const gtsam::SharedDiagonal& model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
Vector b, const gtsam::SharedDiagonal& model);
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
@ -321,38 +373,38 @@ class JacobianFactor {
class HessianFactor {
HessianFactor(const gtsam::HessianFactor& gf);
HessianFactor();
HessianFactor(int j, Matrix G, Vector g, double f);
HessianFactor(int j, Vector mu, Matrix Sigma);
HessianFactor(int j1, int j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
HessianFactor(size_t j, Matrix G, Vector g, double f);
HessianFactor(size_t j, Vector mu, Matrix Sigma);
HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
Vector g2, double f);
HessianFactor(int j1, int j2, int j3, Matrix G11, Matrix G12, Matrix G13,
HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13,
Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
double f);
HessianFactor(const gtsam::GaussianConditional& cg);
HessianFactor(const gtsam::GaussianFactor& factor);
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
};
class GaussianFactorGraph {
GaussianFactorGraph();
GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN);
GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN);
// From FactorGraph
// From FactorGraph
void push_back(gtsam::GaussianFactor* factor);
void print(string s) const;
bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
int size() const;
size_t size() const;
// Building the graph
void add(gtsam::JacobianFactor* factor);
// all these won't work as MATLAB can't handle overloading
// void add(Vector b);
// void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
// void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
// void add(size_t key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
// void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b,
// const gtsam::SharedDiagonal& model);
// void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
// void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3,
// Vector b, const gtsam::SharedDiagonal& model);
// void add(gtsam::HessianFactor* factor);
@ -361,8 +413,9 @@ class GaussianFactorGraph {
double probPrime(const gtsam::VectorValues& c) const;
// combining
static gtsam::GaussianFactorGraph combine2(const gtsam::GaussianFactorGraph& lfg1,
const gtsam::GaussianFactorGraph& lfg2);
static gtsam::GaussianFactorGraph combine2(
const gtsam::GaussianFactorGraph& lfg1,
const gtsam::GaussianFactorGraph& lfg2);
void combine(const gtsam::GaussianFactorGraph& lfg);
// Conversion to matrices
@ -372,11 +425,12 @@ class GaussianFactorGraph {
};
class GaussianSequentialSolver {
GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph, bool useQR);
gtsam::GaussianBayesNet* eliminate() const;
gtsam::VectorValues* optimize() const;
gtsam::GaussianFactor* marginalFactor(int j) const;
Matrix marginalCovariance(int j) const;
GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph,
bool useQR);
gtsam::GaussianBayesNet* eliminate() const;
gtsam::VectorValues* optimize() const;
gtsam::GaussianFactor* marginalFactor(size_t j) const;
Matrix marginalCovariance(size_t j) const;
};
class KalmanFilter {
@ -384,17 +438,17 @@ class KalmanFilter {
// gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
gtsam::GaussianDensity* init(Vector x0, Matrix P0);
void print(string s) const;
static int step(gtsam::GaussianDensity* p);
gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, Matrix B, Vector u,
const gtsam::SharedDiagonal& modelQ);
gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, Matrix B, Vector u,
Matrix Q);
gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, Matrix A1, Vector b,
const gtsam::SharedDiagonal& model);
gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, Vector z,
const gtsam::SharedDiagonal& model);
gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, Vector z,
Matrix Q);
static size_t step(gtsam::GaussianDensity* p);
gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
Matrix B, Vector u, const gtsam::SharedDiagonal& modelQ);
gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F,
Matrix B, Vector u, Matrix Q);
gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0,
Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H,
Vector z, const gtsam::SharedDiagonal& model);
gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H,
Vector z, Matrix Q);
};
//*************************************************************************
@ -402,9 +456,9 @@ class KalmanFilter {
//*************************************************************************
class Symbol {
Symbol(char c, size_t j);
Symbol(char c, size_t j);
void print(string s) const;
size_t key() const;
size_t key() const;
};
class Ordering {
@ -415,24 +469,121 @@ class Ordering {
};
class NonlinearFactorGraph {
NonlinearFactorGraph();
NonlinearFactorGraph();
};
class Values {
Values();
void print(string s) const;
bool exists(size_t j) const;
Values();
size_t size() const;
void print(string s) const;
bool exists(size_t j) const;
};
class Marginals {
Marginals(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& solution);
void print(string s) const;
Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const;
Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution);
void print(string s) const;
Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const;
};
}///\namespace gtsam
//*************************************************************************
// Pose2SLAM
//*************************************************************************
#include <gtsam/slam/pose2SLAM.h>
namespace pose2SLAM {
class Values {
Values();
size_t size() const;
void print(string s) const;
static pose2SLAM::Values Circle(size_t n, double R);
void insertPose(size_t key, const gtsam::Pose2& pose);
gtsam::Pose2 pose(size_t i);
Vector xs() const;
Vector ys() const;
Vector thetas() const;
};
class Graph {
Graph();
// FactorGraph
void print(string s) const;
bool equals(const pose2SLAM::Graph& fg, double tol) const;
size_t size() const;
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
// NonlinearFactorGraph
double error(const pose2SLAM::Values& values) const;
double probPrime(const pose2SLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
const gtsam::Ordering& ordering) const;
// pose2SLAM-specific
void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
};
}///\namespace pose2SLAM
//*************************************************************************
// Pose3SLAM
//*************************************************************************
#include <gtsam/slam/pose3SLAM.h>
namespace pose3SLAM {
class Values {
Values();
size_t size() const;
void print(string s) const;
static pose3SLAM::Values Circle(size_t n, double R);
void insertPose(size_t key, const gtsam::Pose3& pose);
gtsam::Pose3 pose(size_t i);
Vector xs() const;
Vector ys() const;
Vector zs() const;
};
class Graph {
Graph();
// FactorGraph
void print(string s) const;
bool equals(const pose3SLAM::Graph& fg, double tol) const;
size_t size() const;
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
// NonlinearFactorGraph
double error(const pose3SLAM::Values& values) const;
double probPrime(const pose3SLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const pose3SLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const pose3SLAM::Values& values,
const gtsam::Ordering& ordering) const;
// pose3SLAM-specific
void addPrior(size_t key, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::SharedNoiseModel& model);
void addHardConstraint(size_t i, const gtsam::Pose3& p);
pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose3SLAM::Values& solution) const;
};
}///\namespace pose3SLAM
//*************************************************************************
// planarSLAM
//*************************************************************************
@ -442,75 +593,53 @@ namespace planarSLAM {
class Values {
Values();
size_t size() const;
void print(string s) const;
gtsam::Pose2 pose(int key) const;
gtsam::Point2 point(int key) const;
void insertPose(int key, const gtsam::Pose2& pose);
void insertPoint(int key, const gtsam::Point2& point);
void insertPose(size_t key, const gtsam::Pose2& pose);
void insertPoint(size_t key, const gtsam::Point2& point);
gtsam::Pose2 pose(size_t key) const;
gtsam::Point2 point(size_t key) const;
};
class Graph {
Graph();
// FactorGraph
void print(string s) const;
bool equals(const planarSLAM::Graph& fg, double tol) const;
size_t size() const;
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
// NonlinearFactorGraph
double error(const planarSLAM::Values& values) const;
double probPrime(const planarSLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values,
const gtsam::Ordering& ordering) const;
void addPrior(int key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
void addPoseConstraint(int key, const gtsam::Pose2& pose);
void addOdometry(int key1, int key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
void addBearing(int poseKey, int pointKey, const gtsam::Rot2& bearing, const gtsam::SharedNoiseModel& noiseModel);
void addRange(int poseKey, int pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
void addBearingRange(int poseKey, int pointKey, const gtsam::Rot2& bearing, double range,
const gtsam::SharedNoiseModel& noiseModel);
// planarSLAM-specific
void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::SharedNoiseModel& noiseModel);
void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, const gtsam::SharedNoiseModel& noiseModel);
planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
gtsam::Marginals marginals(const planarSLAM::Values& solution) const;
};
class Odometry {
Odometry(int key1, int key2, const gtsam::Pose2& measured,
Odometry(size_t key1, size_t key2, const gtsam::Pose2& measured,
const gtsam::SharedNoiseModel& model);
void print(string s) const;
gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, const gtsam::Ordering& ordering) const;
gtsam::GaussianFactor* linearize(const planarSLAM::Values& center,
const gtsam::Ordering& ordering) const;
};
}///\namespace planarSLAM
//*************************************************************************
// Pose2SLAM
//*************************************************************************
#include <gtsam/slam/pose2SLAM.h>
namespace pose2SLAM {
class Values {
Values();
void print(string s) const;
void insertPose(int key, const gtsam::Pose2& pose);
gtsam::Pose2 pose(int i);
};
class Graph {
Graph();
void print(string s) const;
double error(const pose2SLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
const gtsam::Ordering& ordering) const;
void addPrior(int key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
void addPoseConstraint(int key, const gtsam::Pose2& pose);
void addOdometry(int key1, int key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
};
}///\namespace pose2SLAM
//*************************************************************************
// Simulated2D
//*************************************************************************
@ -520,12 +649,12 @@ namespace simulated2D {
class Values {
Values();
void insertPose(int i, const gtsam::Point2& p);
void insertPoint(int j, const gtsam::Point2& p);
int nrPoses() const;
int nrPoints() const;
gtsam::Point2 pose(int i);
gtsam::Point2 point(int j);
void insertPose(size_t i, const gtsam::Point2& p);
void insertPoint(size_t j, const gtsam::Point2& p);
size_t nrPoses() const;
size_t nrPoints() const;
gtsam::Point2 pose(size_t i);
gtsam::Point2 point(size_t j);
};
class Graph {
@ -542,12 +671,12 @@ namespace simulated2DOriented {
class Values {
Values();
void insertPose(int i, const gtsam::Pose2& p);
void insertPoint(int j, const gtsam::Point2& p);
int nrPoses() const;
int nrPoints() const;
gtsam::Pose2 pose(int i);
gtsam::Point2 point(int j);
void insertPose(size_t i, const gtsam::Pose2& p);
void insertPoint(size_t j, const gtsam::Point2& p);
size_t nrPoses() const;
size_t nrPoints() const;
gtsam::Pose2 pose(size_t i);
gtsam::Point2 point(size_t j);
};
class Graph {
@ -557,3 +686,51 @@ class Graph {
// TODO: add factors, etc.
}///\namespace simulated2DOriented
//*************************************************************************
// VisualSLAM
//*************************************************************************
#include <gtsam/slam/visualSLAM.h>
namespace visualSLAM {
class Values {
Values();
void insertPose(size_t key, const gtsam::Pose3& pose);
void insertPoint(size_t key, const gtsam::Point3& pose);
size_t size() const;
void print(string s) const;
gtsam::Pose3 pose(size_t i);
gtsam::Point3 point(size_t j);
};
class Graph {
Graph();
void print(string s) const;
double error(const visualSLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const visualSLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const visualSLAM::Values& values,
const gtsam::Ordering& ordering) const;
// Measurements
void addMeasurement(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K);
void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::SharedNoiseModel& model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
// Constraints
void addPoseConstraint(size_t poseKey, const gtsam::Pose3& p);
void addPointConstraint(size_t pointKey, const gtsam::Point3& p);
// Priors
void addPosePrior(size_t poseKey, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model);
void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::SharedNoiseModel& model);
void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& model);
visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const visualSLAM::Values& solution) const;
};
}///\namespace visualSLAM

2
gtsam/inference/BayesNet.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file BayesNet
* @file BayesNet.h
* @brief Bayes network
* @author Frank Dellaert
*/

2
gtsam/inference/BayesTree-inl.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file BayesTree.cpp
* @file BayesTree-inl.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert
* @author Michael Kaess

2
gtsam/inference/BayesTree.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file BayesTree
* @file BayesTree.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert
*/

2
gtsam/inference/BayesTreeCliqueBase-inl.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file BayesTreeCliqueBase
* @file BayesTreeCliqueBase-inl.h
* @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert
*/

2
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file BayesTreeCliqueBase
* @file BayesTreeCliqueBase.h
* @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert
*/

2
gtsam/inference/EliminationTree-inl.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file EliminationTree.cpp
* @file EliminationTree-inl.h
* @author Frank Dellaert
* @date Oct 13, 2010
*/

4
gtsam/inference/graph-inl.h
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/*
* graph-inl.h
* @file graph-inl.h
* @brief Graph algorithm using boost library
* @author: Kai Ni
* @author Kai Ni
*/
#pragma once

5
gtsam/inference/inference.cpp
View File

@ -10,9 +10,10 @@
* -------------------------------------------------------------------------- */
/**
* @file inference-inl.h
* @file inference.cpp
* @brief inference definitions
* @author Frank Dellaert, Richard Roberts
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/inference/inference.h>

3
gtsam/inference/inference.h
View File

@ -13,7 +13,8 @@
* @file inference.h
* @brief Contains *generic* inference algorithms that convert between templated
* graphical models, i.e., factor graphs, Bayes nets, and Bayes trees
* @author Frank Dellaert, Richard Roberts
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once

2
gtsam/linear/GaussianBayesTree-inl.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file GaussianBayesTree.cpp
* @file GaussianBayesTree-inl.h
* @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree
* @brief GaussianBayesTree
* @author Frank Dellaert

2
gtsam/linear/GaussianISAM.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file GaussianISAM
* @file GaussianISAM.cpp
* @brief Linear ISAM only
* @author Michael Kaess
*/

2
gtsam/linear/GaussianISAM.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file GaussianISAM
* @file GaussianISAM.h
* @brief Linear ISAM only
* @author Michael Kaess
*/

2
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SequentialSolver.cpp
* @file GaussianSequentialSolver.cpp
* @author Richard Roberts
* @date Oct 19, 2010
*/

2
gtsam/linear/GaussianSequentialSolver.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SequentialSolver.h
* @file GaussianSequentialSolver.h
* @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination.
* @author Richard Roberts
* @date Oct 19, 2010

6
gtsam/linear/IterativeOptimizationParameters.h
View File

@ -96,7 +96,7 @@ struct PreconditionerParameters {
enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */
PreconditionerParameters(): kernel_(CHOLMOD), type_(Combinatorial), verbosity_(SILENT) {}
PreconditionerParameters(): kernel_(GTSAM), type_(Combinatorial), verbosity_(SILENT) {}
PreconditionerParameters(Kernel kernel, const CombinatorialParameters &combinatorial, Verbosity verbosity)
: kernel_(kernel), type_(Combinatorial), combinatorial_(combinatorial), verbosity_(verbosity) {}
@ -180,13 +180,13 @@ public:
PreconditionerParameters preconditioner_;
ConjugateGradientParameters cg_;
enum Kernel { PCG = 0 /*, PCGPlus*/, LSPCG } kernel_ ; /* Iterative Method Kernel */
enum Kernel { PCG = 0, LSPCG } kernel_ ; /* Iterative Method Kernel */
enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */
public:
IterativeOptimizationParameters()
: preconditioner_(), cg_(), kernel_(PCG), verbosity_(SILENT) {}
: preconditioner_(), cg_(), kernel_(LSPCG), verbosity_(SILENT) {}
IterativeOptimizationParameters(const IterativeOptimizationParameters &p) :
preconditioner_(p.preconditioner_), cg_(p.cg_), kernel_(p.kernel_), verbosity_(p.verbosity_) {}

11
gtsam/linear/IterativeSolver.h
View File

@ -9,18 +9,11 @@
* -------------------------------------------------------------------------- */
/**
* @file IterativeSolver.h
* @date Oct 24, 2010
* @author Yong-Dian Jian
* @brief Base Class for all iterative solvers of linear systems
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/IterativeOptimizationParameters.h>
#include <boost/shared_ptr.hpp>
namespace gtsam {
@ -46,7 +39,7 @@ public:
virtual VectorValues::shared_ptr optimize () = 0;
Parameters::shared_ptr parameters() { return parameters_ ; }
inline Parameters::shared_ptr parameters() { return parameters_ ; }
};
}

7
gtsam/linear/KalmanFilter.h
View File

@ -10,11 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file testKalmanFilter.cpp
*
* Simple linear Kalman filter.
* Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
*
* @file KalmanFilter.h
* @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
* @date Sep 3, 2011
* @author Stephen Williams
* @author Frank Dellaert

231
gtsam/linear/SimpleSPCGSolver.cpp Normal file
View File

@ -0,0 +1,231 @@
/* ----------------------------------------------------------------------------
* 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 <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/EliminationTree.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
namespace gtsam {
/* utility function */
std::vector<size_t> extractRowSpec_(const FactorGraph<JacobianFactor>& jfg) {
std::vector<size_t> spec; spec.reserve(jfg.size());
BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) {
spec.push_back(jf->rows());
}
return spec;
}
std::vector<size_t> extractColSpec_(const FactorGraph<GaussianFactor>& gfg, const VariableIndex &index) {
const size_t n = index.size();
std::vector<size_t> spec(n, 0);
for ( Index i = 0 ; i < n ; ++i ) {
const GaussianFactor &gf = *(gfg[index[i].front()]);
for ( GaussianFactor::const_iterator it = gf.begin() ; it != gf.end() ; ++it ) {
spec[*it] = gf.getDim(it);
}
}
return spec;
}
SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr &parameters)
: Base(parameters)
{
std::vector<size_t> colSpec = extractColSpec_(gfg, VariableIndex(gfg));
nVar_ = colSpec.size();
/* Split the factor graph into At (tree) and Ac (constraints)
* Note: This part has to be refined for your graph/application */
GaussianFactorGraph::shared_ptr At;
boost::tie(At, Ac_) = this->splitGraph(gfg);
/* construct row vector spec of the new system */
nAc_ = Ac_->size();
std::vector<size_t> rowSpec = extractRowSpec_(*Ac_);
for ( Index i = 0 ; i < nVar_ ; ++i ) {
rowSpec.push_back(colSpec[i]);
}
/* solve the tree with direct solver. get preconditioner */
Rt_ = EliminationTree<GaussianFactor>::Create(*At)->eliminate(EliminateQR);
xt_ = boost::make_shared<VectorValues>(gtsam::optimize(*Rt_));
/* initial value for the lspcg method */
y0_ = boost::make_shared<VectorValues>(VectorValues::Zero(colSpec));
/* the right hand side of the new system */
by_ = boost::make_shared<VectorValues>(VectorValues::Zero(rowSpec));
for ( Index i = 0 ; i < nAc_ ; ++i ) {
JacobianFactor::shared_ptr jf = (*Ac_)[i];
Vector xi = internal::extractVectorValuesSlices(*xt_, jf->begin(), jf->end());
(*by_)[i] = jf->getb() - jf->getA()*xi;
}
/* allocate buffer for row and column vectors */
tmpY_ = boost::make_shared<VectorValues>(VectorValues::Zero(colSpec));
tmpB_ = boost::make_shared<VectorValues>(VectorValues::Zero(rowSpec));
}
/* implements the CGLS method in Section 7.4 of Bjork's book */
VectorValues::shared_ptr SimpleSPCGSolver::optimize (const VectorValues &initial) {
VectorValues::shared_ptr x(new VectorValues(initial));
VectorValues r = VectorValues::Zero(*by_),
q = VectorValues::Zero(*by_),
p = VectorValues::Zero(*y0_),
s = VectorValues::Zero(*y0_);
residual(*x, r);
transposeMultiply(r, s) ;
p.vector() = s.vector() ;
double gamma = s.vector().squaredNorm(), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ;
const double threshold =
::max(parameters_->epsilon_abs(),
parameters_->epsilon() * parameters_->epsilon() * gamma);
const size_t iMaxIterations = parameters_->maxIterations();
if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR )
cout << "[SimpleSPCGSolver] epsilon = " << parameters_->epsilon()
<< ", max = " << parameters_->maxIterations()
<< ", ||r0|| = " << std::sqrt(gamma)
<< ", threshold = " << threshold << std::endl;
size_t k ;
for ( k = 1 ; k < iMaxIterations ; ++k ) {
multiply(p, q);
alpha = gamma / q.vector().squaredNorm() ;
x->vector() += (alpha * p.vector());
r.vector() -= (alpha * q.vector());
transposeMultiply(r, s);
new_gamma = s.vector().squaredNorm();
beta = new_gamma / gamma ;
p.vector() = s.vector() + beta * p.vector();
gamma = new_gamma ;
if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR) {
cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl;
}
if ( gamma < threshold ) break ;
} // k
if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR )
cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl;
/* transform y back to x */
return this->transform(*x);
}
void SimpleSPCGSolver::residual(const VectorValues &input, VectorValues &output) {
output.vector() = by_->vector();
this->multiply(input, *tmpB_);
axpy(-1.0, *tmpB_, output);
}
void SimpleSPCGSolver::multiply(const VectorValues &input, VectorValues &output) {
this->backSubstitute(input, *tmpY_);
gtsam::multiply(*Ac_, *tmpY_, output);
for ( Index i = 0 ; i < nVar_ ; ++i ) {
output[i + nAc_] = input[i];
}
}
void SimpleSPCGSolver::transposeMultiply(const VectorValues &input, VectorValues &output) {
gtsam::transposeMultiply(*Ac_, input, *tmpY_);
this->transposeBackSubstitute(*tmpY_, output);
for ( Index i = 0 ; i < nVar_ ; ++i ) {
output[i] += input[nAc_+i];
}
}
void SimpleSPCGSolver::backSubstitute(const VectorValues &input, VectorValues &output) {
for ( Index i = 0 ; i < input.size() ; ++i ) {
output[i] = input[i] ;
}
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, *Rt_) {
const Index key = *(cg->beginFrontals());
Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents());
xS = input[key] - cg->get_S() * xS;
output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS);
}
}
void SimpleSPCGSolver::transposeBackSubstitute(const VectorValues &input, VectorValues &output) {
for ( Index i = 0 ; i < input.size() ; ++i ) {
output[i] = input[i] ;
}
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, *Rt_) {
const Index key = *(cg->beginFrontals());
output[key] = gtsam::backSubstituteUpper(output[key], Matrix(cg->get_R()));
const Vector d = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents())
- cg->get_S().transpose() * output[key];
internal::writeVectorValuesSlices(d, output, cg->beginParents(), cg->endParents());
}
}
VectorValues::shared_ptr SimpleSPCGSolver::transform(const VectorValues &y) {
VectorValues::shared_ptr x = boost::make_shared<VectorValues>(VectorValues::Zero(y));
this->backSubstitute(y, *x);
axpy(1.0, *xt_, *x);
return x;
}
boost::tuple<GaussianFactorGraph::shared_ptr, FactorGraph<JacobianFactor>::shared_ptr>
SimpleSPCGSolver::splitGraph(const GaussianFactorGraph &gfg) {
VariableIndex index(gfg);
size_t n = index.size();
std::vector<bool> connected(n, false);
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
FactorGraph<JacobianFactor>::shared_ptr Ac( new FactorGraph<JacobianFactor>());
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
bool augment = false ;
/* check whether this factor should be augmented to the "tree" graph */
if ( gf->keys().size() == 1 ) augment = true;
else {
BOOST_FOREACH ( const Index key, *gf ) {
if ( connected[key] == false ) {
augment = true ;
}
connected[key] = true;
}
}
if ( augment ) At->push_back(gf);
else Ac->push_back(boost::dynamic_pointer_cast<JacobianFactor>(gf));
}
// gfg.print("gfg");
// At->print("At");
// Ac->print("Ac");
return boost::tie(At, Ac);
}
}

91
gtsam/linear/SimpleSPCGSolver.h Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
namespace gtsam {
/**
* This class gives a simple implementation to the SPCG solver presented in Dellaert et al in IROS'10.
*
* Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into
* \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
* \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
* Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
*
* In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it
* with the least-squares variation of the conjugate gradient method.
*
* Note: A full SPCG implementation will come up soon in the next release.
*
* \nosubgrouping
*/
class SimpleSPCGSolver : public IterativeSolver {
public:
typedef IterativeSolver Base;
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
protected:
size_t nVar_ ; ///< number of variables \f$ x \f$
size_t nAc_ ; ///< number of factors in \f$ A_c \f$
FactorGraph<JacobianFactor>::shared_ptr Ac_; ///< the constrained part of the graph
GaussianBayesNet::shared_ptr Rt_; ///< the gaussian bayes net of the tree part of the graph
VectorValues::shared_ptr xt_; ///< the solution of the \f$ A_t^{-1} b_t \f$
VectorValues::shared_ptr y0_; ///< a column zero vector
VectorValues::shared_ptr by_; ///< \f$ [\bar{b_y} ; 0 ] \f$
VectorValues::shared_ptr tmpY_; ///< buffer for the column vectors
VectorValues::shared_ptr tmpB_; ///< buffer for the row vectors
public:
SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr &parameters);
virtual ~SimpleSPCGSolver() {}
virtual VectorValues::shared_ptr optimize () {return optimize(*y0_);}
protected:
VectorValues::shared_ptr optimize (const VectorValues &initial);
/** output = \f$ [\bar{b_y} ; 0 ] - [A_c R_t^{-1} ; I] \f$ input */
void residual(const VectorValues &input, VectorValues &output);
/** output = \f$ [A_c R_t^{-1} ; I] \f$ input */
void multiply(const VectorValues &input, VectorValues &output);
/** output = \f$ [R_t^{-T} A_c^T, I] \f$ input */
void transposeMultiply(const VectorValues &input, VectorValues &output);
/** output = \f$ R_t^{-1} \f$ input */
void backSubstitute(const VectorValues &rhs, VectorValues &sol) ;
/** output = \f$ R_t^{-T} \f$ input */
void transposeBackSubstitute(const VectorValues &rhs, VectorValues &sol) ;
/** return \f$ R_t^{-1} y + x_t \f$ */
VectorValues::shared_ptr transform(const VectorValues &y);
/** naively split a gaussian factor graph into tree and constraint parts
* Note: This function has to be refined for your graph/application */
boost::tuple<GaussianFactorGraph::shared_ptr, FactorGraph<JacobianFactor>::shared_ptr>
splitGraph(const GaussianFactorGraph &gfg);
};
}

8
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -12,14 +12,16 @@
/**
* @file ISAM2-impl.cpp
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
* @author Michael Kaess, Richard Roberts
* @author Michael Kaess
* @author Richard Roberts
*/
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/nonlinear/Symbol.h> // for selective linearization thresholds
#include <gtsam/base/debug.h>
#include <functional>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/base/debug.h>
namespace gtsam {

4
gtsam/nonlinear/ISAM2.h
View File

@ -94,8 +94,8 @@ struct ISAM2Params {
* entries would be added with:
* \code
FastMap<char,Vector> thresholds;
thresholds[PoseKey::chr()] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds[PointKey::chr()] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
params.relinearizeThreshold = thresholds;
\endcode
*/

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/base/cholesky.h> // For NegativeMatrixException
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/SimpleSPCGSolver.h>
using namespace std;
@ -73,7 +74,8 @@ void LevenbergMarquardtOptimizer::iterate() {
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction()));
}
else if ( params_.isCG() ) {
throw runtime_error("todo: ");
SimpleSPCGSolver solver(dampedSystem, *params_.iterativeParams);
delta = *solver.optimize();
}
else {
throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");

5
gtsam/nonlinear/NonlinearFactor.h
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file NoiseModelFactor.h
* @brief Non-linear factor class
* @file NonlinearFactor.h
* @brief Non-linear factor base classes
* @author Frank Dellaert
* @author Richard Roberts
*/
@ -33,7 +33,6 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
/**
* Macro to add a standard clone function to a derived factor

6
gtsam/nonlinear/Ordering.h
View File

@ -17,14 +17,14 @@
#pragma once
#include <set>
#include <gtsam/base/FastMap.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/inference.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/base/FastMap.h>
#include <boost/foreach.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/pool/pool_alloc.hpp>
#include <set>
namespace gtsam {

16
gtsam/nonlinear/SuccessiveLinearizationOptimizer.h
View File

@ -32,8 +32,7 @@ public:
SEQUENTIAL_CHOLESKY,
SEQUENTIAL_QR,
CHOLMOD, /* Experimental Flag */
PCG, /* Experimental Flag */
LSPCG /* Experimental Flag */
CG, /* Experimental Flag */
};
LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
@ -62,11 +61,8 @@ public:
case CHOLMOD:
std::cout << " linear solver type: CHOLMOD\n";
break;
case PCG:
std::cout << " linear solver type: PCG\n";
break;
case LSPCG:
std::cout << " linear solver type: LSPCG\n";
case CG:
std::cout << " linear solver type: CG\n";
break;
default:
std::cout << " linear solver type: (invalid)\n";
@ -94,16 +90,16 @@ public:
}
inline bool isCG() const {
return (linearSolverType == PCG || linearSolverType == LSPCG);
return (linearSolverType == CG);
}
GaussianFactorGraph::Eliminate getEliminationFunction() {
switch (linearSolverType) {
case MULTIFRONTAL_CHOLESKY:
case MULTIFRONTAL_QR:
case SEQUENTIAL_CHOLESKY:
return EliminatePreferCholesky;
case SEQUENTIAL_CHOLESKY:
case MULTIFRONTAL_QR:
case SEQUENTIAL_QR:
return EliminateQR;

5
gtsam/nonlinear/tests/testOrdering.cpp
View File

@ -14,9 +14,10 @@
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;

3
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -16,11 +16,12 @@
* @date Feb 7, 2012
*/
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>

31
gtsam/nonlinear/tests/testValues.cpp
View File

@ -10,28 +10,33 @@
* -------------------------------------------------------------------------- */
/**
* @file testDynamicValues.cpp
* @file testValues.cpp
* @author Richard Roberts
*/
#include <CppUnitLite/TestHarness.h>
#include <stdexcept>
#include <limits>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <stdexcept>
#include <limits>
using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
const Key key1(Symbol('v',1)), key2(Symbol('v',2)), key3(Symbol('v',3)), key4(Symbol('v',4));
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
const Symbol key1('v',1), key2('v',2), key3('v',3), key4('v',4);
/* ************************************************************************* */
TEST( Values, equals1 )
@ -332,9 +337,9 @@ TEST(Values, Symbol_filter) {
Pose3 pose3(Pose2(0.3, 0.7, 0.9));
Values values;
values.insert(Symbol('x',0), pose0);
values.insert(X(0), pose0);
values.insert(Symbol('y',1), pose1);
values.insert(Symbol('x',2), pose2);
values.insert(X(2), pose2);
values.insert(Symbol('y',3), pose3);
int i = 0;

2
gtsam/slam/BearingRangeFactor.h
View File

@ -66,7 +66,7 @@ namespace gtsam {
<< keyFormatter(this->key2()) << ")\n";
measuredBearing_.print(" measured bearing");
std::cout << " measured range: " << measuredRange_ << std::endl;
this->noiseModel_->print(" noise model");
this->noiseModel_->print(" noise model: ");
}
/** equals */

2
gtsam/slam/BetweenFactor.h
View File

@ -72,7 +72,7 @@ namespace gtsam {
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measured_.print(" measured");
this->noiseModel_->print(" noise model");
this->noiseModel_->print(" noise model: ");
}
/** equals */

5
gtsam/slam/GeneralSFMFactor.h
View File

@ -20,9 +20,10 @@
#pragma once
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
namespace gtsam {

2
gtsam/slam/PriorFactor.h
View File

@ -64,7 +64,7 @@ namespace gtsam {
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "PriorFactor(" << keyFormatter(this->key()) << ")\n";
prior_.print(" prior");
this->noiseModel_->print(" noise model");
this->noiseModel_->print(" noise model: ");
}
/** equals */

342
gtsam/slam/dataset.cpp
View File

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

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