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

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

Conflicts:
	gtsam/inference/FactorGraph-inl.h
	gtsam/inference/tests/testFactorGraph.cpp
	matlab/CMakeLists.txt
release/4.3a0
Richard Roberts 2012-09-06 14:44:14 +00:00
parent 91559d3c25 6d1b86c2e0
commit aecf50735a
80 changed files with 1717 additions and 2073 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

354
.cproject
View File

@ -309,14 +309,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -343,6 +335,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -350,6 +343,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -397,6 +391,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -404,6 +399,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -411,6 +407,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -426,11 +423,20 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -519,22 +525,6 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="CppUnitLite" 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="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -551,6 +541,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="CppUnitLite" 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="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -575,26 +581,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testValues.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testOrdering.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testKey.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -679,26 +685,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testValues.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -985,6 +991,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -992,6 +999,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -999,6 +1007,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1028,6 +1037,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testIterative.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testIterative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSubgraphSolver.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSubgraphSolver.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraphB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianFactorGraphB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1134,6 +1167,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1179,10 +1213,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testLinearContainerFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testLinearContainerFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1267,10 +1301,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearContainerFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testLinearContainerFactor.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1605,7 +1639,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1645,7 +1678,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1653,7 +1685,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1845,7 +1876,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1867,102 +1897,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments>
@ -2164,6 +2098,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2171,6 +2106,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2178,6 +2114,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2185,6 +2122,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2318,34 +2256,98 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWrap.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -2389,6 +2391,38 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cproject>

12
CMakeLists.txt
View File

@ -161,7 +161,7 @@ if (DOXYGEN_FOUND)
add_subdirectory(doc)
endif()
# Set up CPack
## Set up CPack
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")
set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu")
@ -171,12 +171,18 @@ set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR})
set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH})
set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}")
set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory
set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makestats.sh$")
#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory
#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error
set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$")
set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/")
set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/")
set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs
# Deb-package specific cpack
set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
# print configuration variables
message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================")

6
README
View File

@ -59,10 +59,12 @@ Tested compilers
- GCC 4.2-4.7
- Clang 2.9-3.2
- OSX GCC 4.2
- MSVC 2010, 2012
Tested systems:
- Ubuntu 11.04, 11.10, 12.04
- MacOS 10.6, 10.7
- Windows 7
2)
GTSAM makes extensive use of debug assertions, and we highly recommend you work
@ -72,8 +74,8 @@ will run up to 10x faster in Release mode! See the end of this document for
additional debugging tips.
3)
GTSAM has Doxygen documentation. To generate, run the the script
makedoc.sh.
GTSAM has Doxygen documentation. To generate, run 'make doc' from your
build directory.
4)
The instructions below install the library to the default system install path and

12
examples/Pose2SLAMwSPCG.cpp
View File

@ -54,13 +54,9 @@
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
// ???
#include <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
@ -110,14 +106,6 @@ int main(int argc, char** argv) {
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
{
parameters.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
cout << "simple spcg solver final error = " << graph.error(result) << endl;
}
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);

49
gtsam.h
View File

@ -1022,8 +1022,10 @@ class GaussianFactorGraph {
// Conversion to matrices
Matrix sparseJacobian_() const;
Matrix denseJacobian() const;
Matrix denseHessian() const;
Matrix augmentedJacobian() const;
pair<Matrix,Vector> jacobian() const;
Matrix augmentedHessian() const;
pair<Matrix,Vector> hessian() const;
};
class GaussianISAM {
@ -1045,6 +1047,46 @@ class GaussianSequentialSolver {
Matrix marginalCovariance(size_t j) const;
};
#include <gtsam/linear/IterativeSolver.h>
virtual class IterativeOptimizationParameters {
string getKernel() const ;
string getVerbosity() const;
void setKernel(string s) ;
void setVerbosity(string s) ;
};
//virtual class IterativeSolver {
// IterativeSolver();
// gtsam::VectorValues optimize ();
//};
#include <gtsam/linear/ConjugateGradientSolver.h>
virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters {
ConjugateGradientParameters();
size_t getMinIterations() const ;
size_t getMaxIterations() const ;
size_t getReset() const;
double getEpsilon_rel() const;
double getEpsilon_abs() const;
void setMinIterations(size_t value);
void setMaxIterations(size_t value);
void setReset(size_t value);
void setEpsilon_rel(double value);
void setEpsilon_abs(double value);
};
#include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
SubgraphSolverParameters();
void print(string s) const;
};
class SubgraphSolver {
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters);
gtsam::VectorValues optimize() const;
};
#include <gtsam/linear/KalmanFilter.h>
class KalmanFilter {
KalmanFilter(size_t n);
@ -1288,6 +1330,7 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams {
void setLinearSolverType(string solver);
void setOrdering(const gtsam::Ordering& ordering);
void setIterativeParams(const gtsam::SubgraphSolverParameters &params);
bool isMultifrontal() const;
bool isSequential() const;
@ -1443,6 +1486,8 @@ class ISAM2 {
bool equals(const gtsam::ISAM2& other, double tol) const;
void print(string s) const;
void printStats() const;
void saveGraph(string s) const;
gtsam::ISAM2Result update();
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);

2
gtsam_unstable/base/DSFVector.cpp → gtsam/base/DSFVector.cpp
View File

@ -18,7 +18,7 @@
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <gtsam_unstable/base/DSFVector.h>
#include <gtsam/base/DSFVector.h>
using namespace std;

0
gtsam_unstable/base/DSFVector.h → gtsam/base/DSFVector.h
View File

21
gtsam/base/Matrix.h
View File

@ -67,11 +67,30 @@ inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);}
*/
Matrix Matrix_(size_t m, size_t n, ...);
// Matlab-like syntax
/**
* MATLAB like constructors
* Creates an zeros matrix, with matlab-like syntax
*
* Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros,
* don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error.
*/
Matrix zeros(size_t m, size_t n);
/**
* Creates an identity matrix, with matlab-like syntax
*
* Note: if assigning a block (created from an Eigen block() function) of a matrix to identity,
* don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error.
*/
Matrix eye(size_t m, size_t n);
/**
* Creates a square identity matrix, with matlab-like syntax
*
* Note: if assigning a block (created from an Eigen block() function) of a matrix to identity,
* don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error.
*/
inline Matrix eye( size_t m ) { return eye(m,m); }
Matrix diag(const Vector& v);

2
gtsam_unstable/base/tests/testDSFVector.cpp → gtsam/base/tests/testDSFVector.cpp
View File

@ -24,7 +24,7 @@
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/base/DSFVector.h>
#include <gtsam/base/DSFVector.h>
using namespace std;
using namespace gtsam;

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

@ -100,7 +100,7 @@ namespace gtsam {
++ firstIndex;
// Check that number of variables is in bounds
if(firstIndex + nFrontals >= variableIndex.size())
if(firstIndex + nFrontals > variableIndex.size())
throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph.");
// Get set of involved factors

59
gtsam/inference/tests/testFactorGraph.cpp
View File

@ -60,65 +60,6 @@ TEST(FactorGraph, eliminateFrontals) {
EXPECT(assert_equal(expectedCond, *actualCond));
}
///* ************************************************************************* */
// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree )
//{
// SymbolicFactorGraph G;
// G.push_factor("x1", "x2");
// G.push_factor("x1", "x3");
// G.push_factor("x1", "x4");
// G.push_factor("x2", "x3");
// G.push_factor("x2", "x4");
// G.push_factor("x3", "x4");
//
// SymbolicFactorGraph T, C;
// boost::tie(T, C) = G.splitMinimumSpanningTree();
//
// SymbolicFactorGraph expectedT, expectedC;
// expectedT.push_factor("x1", "x2");
// expectedT.push_factor("x1", "x3");
// expectedT.push_factor("x1", "x4");
// expectedC.push_factor("x2", "x3");
// expectedC.push_factor("x2", "x4");
// expectedC.push_factor("x3", "x4");
// CHECK(assert_equal(expectedT,T));
// CHECK(assert_equal(expectedC,C));
//}
///* ************************************************************************* */
///**
// * x1 - x2 - x3 - x4 - x5
// * | | / |
// * l1 l2 l3
// */
// SL-FIX TEST( FactorGraph, removeSingletons )
//{
// SymbolicFactorGraph G;
// G.push_factor("x1", "x2");
// G.push_factor("x2", "x3");
// G.push_factor("x3", "x4");
// G.push_factor("x4", "x5");
// G.push_factor("x2", "l1");
// G.push_factor("x3", "l2");
// G.push_factor("x4", "l2");
// G.push_factor("x4", "l3");
//
// SymbolicFactorGraph singletonGraph;
// set<Symbol> singletons;
// boost::tie(singletonGraph, singletons) = G.removeSingletons();
//
// set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
// CHECK(singletons_excepted == singletons);
//
// SymbolicFactorGraph singletonGraph_excepted;
// singletonGraph_excepted.push_factor("x2", "l1");
// singletonGraph_excepted.push_factor("x4", "l3");
// singletonGraph_excepted.push_factor("x1", "x2");
// singletonGraph_excepted.push_factor("x4", "x5");
// singletonGraph_excepted.push_factor("x2", "x3");
// CHECK(singletonGraph_excepted.equals(singletonGraph));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

32
gtsam/linear/ConjugateGradientSolver.h
View File

@ -20,9 +20,7 @@ namespace gtsam {
*/
class ConjugateGradientParameters : public IterativeOptimizationParameters {
public:
typedef IterativeOptimizationParameters Base;
typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
@ -49,7 +47,21 @@ public:
inline double epsilon_rel() const { return epsilon_rel_; }
inline double epsilon_abs() const { return epsilon_abs_; }
void print() const {
inline size_t getMinIterations() const { return minIterations_; }
inline size_t getMaxIterations() const { return maxIterations_; }
inline size_t getReset() const { return reset_; }
inline double getEpsilon() const { return epsilon_rel_; }
inline double getEpsilon_rel() const { return epsilon_rel_; }
inline double getEpsilon_abs() const { return epsilon_abs_; }
inline void setMinIterations(size_t value) { minIterations_ = value; }
inline void setMaxIterations(size_t value) { maxIterations_ = value; }
inline void setReset(size_t value) { reset_ = value; }
inline void setEpsilon(double value) { epsilon_rel_ = value; }
inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
virtual void print(const std::string &s="") const {
Base::print();
std::cout << "ConjugateGradientParameters: "
<< "minIter = " << minIterations_
@ -61,18 +73,4 @@ public:
}
};
//class ConjugateGradientSolver : public IterativeSolver {
//
//public:
//
// typedef ConjugateGradientParameters Parameters;
//
// Parameters parameters_;
//
// ConjugateGradientSolver(const ConjugateGradientParameters &parameters) : parameters_(parameters) {}
// virtual VectorValues::shared_ptr optimize () = 0;
// virtual const IterativeOptimizationParameters& _params() const = 0;
//};
}

6
gtsam/linear/GaussianBayesNet.cpp
View File

@ -16,7 +16,7 @@
*/
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/BayesNet-inl.h>
@ -242,12 +242,12 @@ double determinant(const GaussianBayesNet& bayesNet) {
/* ************************************************************************* */
VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) {
return gradient(FactorGraph<JacobianFactor>(bayesNet), x0);
return gradient(GaussianFactorGraph(bayesNet), x0);
}
/* ************************************************************************* */
void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) {
gradientAtZero(FactorGraph<JacobianFactor>(bayesNet), g);
gradientAtZero(GaussianFactorGraph(bayesNet), g);
}
/* ************************************************************************* */

2
gtsam/linear/GaussianBayesTree.cpp
View File

@ -18,7 +18,7 @@
*/
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
namespace gtsam {

146
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -166,7 +166,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::denseJacobian() const {
Matrix GaussianFactorGraph::augmentedJacobian() const {
// Convert to Jacobians
FactorGraph<JacobianFactor> jfg;
jfg.reserve(this->size());
@ -182,6 +182,14 @@ namespace gtsam {
return combined.matrix_augmented();
}
/* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraph::jacobian() const {
Matrix augmented = augmentedJacobian();
return make_pair(
augmented.leftCols(augmented.cols()-1),
augmented.col(augmented.cols()-1));
}
/* ************************************************************************* */
// Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) {
@ -317,9 +325,7 @@ break;
}
/* ************************************************************************* */
static
FastMap<Index, SlotEntry> findScatterAndDims
(const FactorGraph<GaussianFactor>& factors) {
static FastMap<Index, SlotEntry> findScatterAndDims(const FactorGraph<GaussianFactor>& factors) {
const bool debug = ISDEBUG("findScatterAndDims");
@ -349,7 +355,7 @@ break;
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::denseHessian() const {
Matrix GaussianFactorGraph::augmentedHessian() const {
Scatter scatter = findScatterAndDims(*this);
@ -367,6 +373,14 @@ break;
return result;
}
/* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraph::hessian() const {
Matrix augmented = augmentedHessian();
return make_pair(
augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
augmented.col(augmented.rows()-1).head(augmented.rows()-1));
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals) {
@ -507,4 +521,126 @@ break;
} // \EliminatePreferCholesky
/* ************************************************************************* */
static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !result ) {
result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
}
return result;
}
/* ************************************************************************* */
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back((*Ai)*x);
}
return e;
}
/* ************************************************************************* */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
/* ************************************************************************* */
VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0);
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(Ai->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
}
/* ************************************************************************* */
void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) {
// Zero-out the gradient
g.setZero();
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(-Ai->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
/* ************************************************************************* */
void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
r[i] = Ai->getb();
i++;
}
VectorValues Ax = VectorValues::SameStructure(r);
multiply(fg,x,Ax);
axpy(-1.0,Ax,r);
}
/* ************************************************************************* */
void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
SubVector &y = r[i];
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
y += Ai->getA(j) * x[*j];
}
++i;
}
}
/* ************************************************************************* */
void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
x[*j] += Ai->getA(j).transpose() * r[i];
}
++i;
}
}
/* ************************************************************************* */
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e->push_back(Ai->error_vector(x));
}
return e;
}
} // namespace gtsam

106
gtsam/linear/GaussianFactorGraph.h
View File

@ -31,24 +31,6 @@
namespace gtsam {
/** return A*x-b
* \todo Make this a member function - affects SubgraphPreconditioner */
template<class FACTOR>
Errors gaussianErrors(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
return *gaussianErrors_(fg, x);
}
/** shared pointer version
* \todo Make this a member function - affects SubgraphPreconditioner */
template<class FACTOR>
boost::shared_ptr<Errors> gaussianErrors_(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, fg) {
e->push_back(factor->error_vector(x));
}
return e;
}
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == GaussianFactor
@ -188,15 +170,43 @@ namespace gtsam {
Matrix sparseJacobian_() const;
/**
* Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b
* with standard deviations are baked into A and b
* Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
* Jacobian matrix, augmented with b with the noise models baked
* into A and b. The negative log-likelihood is
* \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
* GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
*/
Matrix denseJacobian() const;
Matrix augmentedJacobian() const;
/**
* Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
* with the noise models baked into A and b. The negative log-likelihood
* is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
* GaussianFactorGraph::augmentedJacobian and
* GaussianFactorGraph::sparseJacobian.
*/
std::pair<Matrix,Vector> jacobian() const;
/**
* Return a dense \f$ n \times n \f$ Hessian matrix, augmented with \f$ A^T b \f$
* Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
* matrix, augmented with the information vector \f$ \eta \f$. The
* augmented Hessian is
\f[ \left[ \begin{array}{ccc}
\Lambda & \eta \\
\eta^T & c
\end{array} \right] \f]
and the negative log-likelihood is
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
*/
Matrix denseHessian() const;
Matrix augmentedHessian() const;
/**
* Return the dense Hessian \f$ \Lambda \f$ and information vector
* \f$ \eta \f$, with the noise models baked in. The negative log-likelihood
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
* GaussianFactorGraph::augmentedHessian.
*/
std::pair<Matrix,Vector> hessian() const;
private:
/** Serialization function */
@ -294,4 +304,54 @@ namespace gtsam {
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
/****** Linear Algebra Opeations ******/
/** return A*x */
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x);
/** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e);
/** In-place version e <- A*x that takes an iterator. */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
/** x += alpha*A'*e */
void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around zero.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g);
/* matrix-vector operations */
void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x);
/** shared pointer version
* \todo Make this a member function - affects SubgraphPreconditioner */
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x);
/** return A*x-b
* \todo Make this a member function - affects SubgraphPreconditioner */
inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) {
return *gaussianErrors_(fg, x); }
} // namespace gtsam

22
gtsam/linear/HessianFactor.cpp
View File

@ -261,12 +261,10 @@ HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
if (debug) cout << "Combining " << factors.size() << " factors" << endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
{
shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
updateATA(*hessian, scatter);
else {
JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (jacobianFactor)
if(factor) {
if(shared_ptr hessian = boost::dynamic_pointer_cast<HessianFactor>(factor))
updateATA(*hessian, scatter);
else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(factor))
updateATA(*jacobianFactor, scatter);
else
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
@ -309,16 +307,6 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
}
}
/* ************************************************************************* */
double HessianFactor::constantTerm() const {
return info_(this->size(), this->size())(0,0);
}
/* ************************************************************************* */
HessianFactor::constColumn HessianFactor::linearTerm() const {
return info_.rangeColumn(0, this->size(), this->size(), 0);
}
/* ************************************************************************* */
Matrix HessianFactor::computeInformation() const {
return info_.full().selfadjointView<Eigen::Upper>();
@ -360,7 +348,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
// Apply updates to the upper triangle
tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(size_t j1=0; j1<=j2; ++j1) {
@ -437,7 +424,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
// Apply updates to the upper triangle
tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size());
for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(size_t j1=0; j1<=j2; ++j1) {

65
gtsam/linear/HessianFactor.h
View File

@ -120,20 +120,6 @@ namespace gtsam {
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
BlockInfo info_; ///< The block view of the full information matrix.
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
public:
typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
@ -256,16 +242,39 @@ namespace gtsam {
*/
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
/** Return a view of the block at (j1,j2) of the <em>upper-triangular part</em> of the
* information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation
* above to explain that only the upper-triangular part of the information matrix is stored
* and returned by this function.
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return A view of the requested block, not a copy.
*/
Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); }
/** Return the <em>upper-triangular part</em> of the full *augmented* information matrix,
* as described above. See HessianFactor class documentation above to explain that only the
* upper-triangular part of the information matrix is stored and returned by this function.
*/
constBlock info() const { return info_.full(); }
/** Return the <em>upper-triangular part</em> of the full *augmented* information matrix,
* as described above. See HessianFactor class documentation above to explain that only the
* upper-triangular part of the information matrix is stored and returned by this function.
*/
Block info() { return info_.full(); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/
double constantTerm() const;
double constantTerm() const { return info_(this->size(), this->size())(0,0); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/
double& constantTerm() { return info_(this->size(), this->size())(0,0); }
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
@ -273,9 +282,19 @@ namespace gtsam {
* @return The linear term \f$ g \f$ */
constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return The linear term \f$ g \f$ */
Column linearTerm(iterator j) { return info_.column(j-begin(), size(), 0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
constColumn linearTerm() const;
constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
Column linearTerm() { return info_.rangeColumn(0, this->size(), this->size(), 0); }
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
@ -315,6 +334,20 @@ namespace gtsam {
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
/** assert invariants */
void assertInvariants() const;

50
gtsam/linear/IterativeSolver.cpp Normal file
View File

@ -0,0 +1,50 @@
/**
* @file IterativeSolver.cpp
* @brief
* @date Sep 3, 2012
* @author Yong-Dian Jian
*/
#include <gtsam/linear/IterativeSolver.h>
#include <boost/algorithm/string.hpp>
#include <string>
namespace gtsam {
std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); }
std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); }
void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); }
IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "CG") return IterativeOptimizationParameters::CG;
/* default is cg */
else return IterativeOptimizationParameters::CG;
}
IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "SILENT") return IterativeOptimizationParameters::SILENT;
else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY;
else if (s == "ERROR") return IterativeOptimizationParameters::ERROR;
/* default is default */
else return IterativeOptimizationParameters::SILENT;
}
std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k) {
if ( k == CG ) return "CG";
else return "UNKNOWN";
}
std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) {
if (verbosity == SILENT) return "SILENT";
else if (verbosity == COMPLEXITY) return "COMPLEXITY";
else if (verbosity == ERROR) return "ERROR";
else return "UNKNOWN";
}
}

22
gtsam/linear/IterativeSolver.h
View File

@ -13,6 +13,7 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <string>
namespace gtsam {
@ -41,23 +42,34 @@ namespace gtsam {
inline Kernel kernel() const { return kernel_; }
inline Verbosity verbosity() const { return verbosity_; }
/* matlab interface */
std::string getKernel() const ;
std::string getVerbosity() const;
void setKernel(const std::string &s) ;
void setVerbosity(const std::string &s) ;
void print() const {
const std::string kernelStr[1] = {"cg"};
std::cout << "IterativeOptimizationParameters: "
<< "kernel = " << kernelStr[kernel_]
<< ", verbosity = " << verbosity_ << std::endl;
<< "kernel = " << kernelTranslator(kernel_)
<< ", verbosity = " << verbosityTranslator(verbosity_) << std::endl;
}
static Kernel kernelTranslator(const std::string &s);
static Verbosity verbosityTranslator(const std::string &s);
static std::string kernelTranslator(Kernel k);
static std::string verbosityTranslator(Verbosity v);
};
class IterativeSolver {
public:
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
IterativeSolver(){}
IterativeSolver() {}
virtual ~IterativeSolver() {}
/* interface to the nonlinear optimizer */
virtual VectorValues optimize () = 0;
/* update interface to the nonlinear optimizer */
virtual void replaceFactors(const GaussianFactorGraph::shared_ptr &factorGraph, const double lambda) {}
virtual VectorValues optimize () = 0;
};
}

1
gtsam/linear/JacobianFactor.h
View File

@ -322,7 +322,6 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(model_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
}; // JacobianFactor
} // gtsam

122
gtsam/linear/JacobianFactorGraph.cpp
View File

@ -1,122 +0,0 @@
/**
* @file JacobianFactorGraph.h
* @date Jun 6, 2012
* @author Yong Dian Jian
*/
#include <gtsam/linear/JacobianFactorGraph.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
e.push_back((*Ai)*x);
}
return e;
}
/* ************************************************************************* */
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
/* ************************************************************************* */
VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0);
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(factor->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
}
/* ************************************************************************* */
void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g) {
// Zero-out the gradient
g.setZero();
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(-factor->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
/* ************************************************************************* */
void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
r[i] = factor->getb();
i++;
}
VectorValues Ax = VectorValues::SameStructure(r);
multiply(fg,x,Ax);
axpy(-1.0,Ax,r);
}
/* ************************************************************************* */
void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
SubVector &y = r[i];
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
y += factor->getA(j) * x[*j];
}
++i;
}
}
/* ************************************************************************* */
void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
x[*j] += factor->getA(j).transpose() * r[i];
}
++i;
}
}
/* ************************************************************************* */
JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg) {
JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph());
jfg->reserve(gfg.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
JacobianFactor::shared_ptr castedFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if(castedFactor) jfg->push_back(castedFactor);
else throw std::invalid_argument("dynamicCastFactors(), dynamic_cast failed, meaning an invalid cast was requested.");
}
return jfg;
}
} // namespace

73
gtsam/linear/JacobianFactorGraph.h
View File

@ -1,73 +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 JacobianFactorGraph.cpp
* @date Jun 6, 2012
* @brief Linear Algebra Operations for a JacobianFactorGraph
* @author Yong Dian Jian
*/
#pragma once
#include <gtsam/linear/Errors.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
namespace gtsam {
typedef FactorGraph<JacobianFactor> JacobianFactorGraph;
/** return A*x */
Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x);
/** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e);
/** In-place version e <- A*x that takes an iterator. */
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
/** x += alpha*A'*e */
void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around zero.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g);
/* matrix-vector operations */
void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x);
/** dynamic_cast the gaussian factors down to jacobian factors */
JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg);
}

226
gtsam/linear/SimpleSPCGSolver.cpp
View File

@ -1,226 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/SimpleSPCGSolver.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 &parameters)
: Base(), parameters_(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 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 = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
const size_t iMaxIterations = parameters_.maxIterations();
const Parameters::Verbosity verbosity = parameters_.verbosity();
if ( verbosity >= ConjugateGradientParameters::ERROR )
std::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 ( verbosity >= ConjugateGradientParameters::ERROR) {
std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::endl;
}
if ( gamma < threshold ) break ;
} // k
if ( verbosity >= ConjugateGradientParameters::ERROR )
std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::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 SimpleSPCGSolver::transform(const VectorValues &y) {
VectorValues x = 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));
}
return boost::tie(At, Ac);
}
}

98
gtsam/linear/SimpleSPCGSolver.h
View File

@ -1,98 +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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/JacobianFactor.h>
namespace gtsam {
struct SimpleSPCGSolverParameters : public ConjugateGradientParameters {
typedef ConjugateGradientParameters Base;
SimpleSPCGSolverParameters() : Base() {}
};
/**
* 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 SimpleSPCGSolverParameters Parameters;
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
Parameters parameters_; ///< Parameters for iterative method
public:
SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters &parameters);
virtual ~SimpleSPCGSolver() {}
virtual VectorValues optimize () {return optimize(*y0_);}
protected:
VectorValues 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 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);
};
}

19
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -17,18 +17,29 @@
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <boost/foreach.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
}
result->push_back(jf);
}
return result;
}
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2,
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar) :
Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
}
/* ************************************************************************* */

27
gtsam/linear/SubgraphPreconditioner.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
namespace gtsam {
@ -32,15 +33,14 @@ namespace gtsam {
class SubgraphPreconditioner {
public:
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
typedef boost::shared_ptr<const FactorGraph<JacobianFactor> > sharedFG;
typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
typedef boost::shared_ptr<const VectorValues> sharedValues;
typedef boost::shared_ptr<const Errors> sharedErrors;
private:
sharedFG Ab1_, Ab2_;
sharedFG Ab2_;
sharedBayesNet Rc1_;
sharedValues xbar_; ///< A1 \ b1
sharedErrors b2bar_; ///< A2*xbar - b2
@ -48,17 +48,14 @@ namespace gtsam {
public:
SubgraphPreconditioner();
/**
* Constructor
* @param Ab1: the Graph A1*x=b1
* @param Ab2: the Graph A2*x=b2
* @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to R1*x=c1
*/
SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
/** Access Ab1 */
const sharedFG& Ab1() const { return Ab1_; }
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
/** Access Ab2 */
const sharedFG& Ab2() const { return Ab2_; }
@ -69,23 +66,23 @@ namespace gtsam {
/** Access b2bar */
const sharedErrors b2bar() const { return b2bar_; }
/**
* Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian
*/
// SubgraphPreconditioner add_priors(double sigma) const;
/**
* Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian
*/
// SubgraphPreconditioner add_priors(double sigma) const;
/* x = xbar + inv(R1)*y */
VectorValues x(const VectorValues& y) const;
/* A zero VectorValues with the structure of xbar */
VectorValues zero() const {
VectorValues V(VectorValues::Zero(*xbar_)) ;
VectorValues V(VectorValues::Zero(*xbar_));
return V ;
}
/**
* Add constraint part of the error only, used in both calls above
* Add constraint part of the error only
* y += alpha*inv(R1')*A2'*e2
* Takes a range indicating e2 !!!!
*/

80
gtsam/linear/SubgraphSolver-inl.h
View File

@ -1,80 +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
//
// * -------------------------------------------------------------------------- */
//
//#pragma once
//
//#include <boost/foreach.hpp>
//
//#include <gtsam_unstable/linear/iterative-inl.h>
//#include <gtsam/inference/graph-inl.h>
//#include <gtsam/inference/EliminationTree.h>
//
//namespace gtsam {
//
//template<class GRAPH, class LINEAR, class KEY>
//void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
//
// GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
// GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
//
// if (parameters_->verbosity()) cout << "split the graph ...";
// split(pairs_, *graph, *Ab1, *Ab2) ;
// if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
//
// // // Add a HardConstraint to the root, otherwise the root will be singular
// // Key root = keys.back();
// // T_.addHardConstraint(root, theta0[root]);
// //
// // // compose the approximate solution
// // theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
//
// LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!!
// SubgraphPreconditioner::sharedBayesNet Rc1 =
// EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate(&EliminateQR);
// SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
//
// pc_ = boost::make_shared<SubgraphPreconditioner>(
// Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
//}
//
//template<class GRAPH, class LINEAR, class KEY>
//VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
//
// // preconditioned conjugate gradient
// VectorValues zeros = pc_->zero();
// VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>
// (*pc_, zeros, *parameters_);
//
// boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
// *xbar = pc_->x(ybar);
// return xbar;
//}
//
//template<class GRAPH, class LINEAR, class KEY>
//void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values& theta0) {
// // generate spanning tree
// PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
//
// // make the ordering
// list<KEY> keys = predecessorMap2Keys(tree_);
// ordering_ = boost::make_shared<Ordering>(list<Key>(keys.begin(), keys.end()));
//
// // build factor pairs
// pairs_.clear();
// typedef pair<KEY,KEY> EG ;
// BOOST_FOREACH( const EG &eg, tree_ ) {
// Key key1 = Key(eg.first),
// key2 = Key(eg.second) ;
// pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
// }
//}
//
//} // \namespace gtsam

124
gtsam/linear/SubgraphSolver.cpp
View File

@ -14,100 +14,119 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/base/DSFVector.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <list>
using namespace std;
namespace gtsam {
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters)
: parameters_(parameters)
{
initialize(gfg);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters)
: parameters_(parameters)
{
initialize(*jfg);
}
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters)
: parameters_(parameters) {
boost::tie(Ab1, Ab2) = splitGraph(gfg) ;
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1)->eliminate(&EliminateQR);
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}
if (parameters_.verbosity())
cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
// // Add a HardConstraint to the root, otherwise the root will be singular
// Key root = keys.back();
// T_.addHardConstraint(root, theta0[root]);
//
// // compose the approximate solution
// theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters)
: parameters_(parameters) {
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
initialize(Rc1, Ab2);
}
// Convert or cast Ab1 to JacobianFactors
boost::shared_ptr<FactorGraph<JacobianFactor> > Ab1Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
Ab1Jacobians->reserve(Ab1->size());
BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab1) {
if(boost::shared_ptr<JacobianFactor> jf =
boost::dynamic_pointer_cast<JacobianFactor>(factor))
Ab1Jacobians->push_back(jf);
else
Ab1Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
}
// Convert or cast Ab2 to JacobianFactors
boost::shared_ptr<FactorGraph<JacobianFactor> > Ab2Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
Ab1Jacobians->reserve(Ab2->size());
BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab2) {
if(boost::shared_ptr<JacobianFactor> jf =
boost::dynamic_pointer_cast<JacobianFactor>(factor))
Ab2Jacobians->push_back(jf);
else
Ab2Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
const Parameters &parameters) : parameters_(parameters)
{
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab1Jacobians, Ab2Jacobians, Rc1, xbar);
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters)
{
initialize(Rc1, Ab2);
}
VectorValues SubgraphSolver::optimize() {
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_);
return pc_->x(ybar);
}
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) {
void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
{
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
Ab2 = boost::make_shared<GaussianFactorGraph>();
VariableIndex index(gfg);
size_t n = index.size();
std::vector<bool> connected(n, false);
boost::tie(Ab1, Ab2) = splitGraph(jfg) ;
if (parameters_.verbosity())
cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl;
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2)
{
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
const VariableIndex index(jfg);
const size_t n = index.size();
DSFVector D(n);
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph());
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
size_t t = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
if ( gf->keys().size() > 2 ) {
throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
}
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;
}
const Index u = gf->keys()[0], v = gf->keys()[1],
u_root = D.findSet(u), v_root = D.findSet(v);
if ( u_root != v_root ) {
t++; augment = true ;
D.makeUnionInPlace(u_root, v_root);
}
}
if ( augment ) At->push_back(gf);
else Ac->push_back(gf);
}
@ -115,7 +134,4 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) {
return boost::tie(At, Ac);
}
} // \namespace gtsam

40
gtsam/linear/SubgraphSolver.h
View File

@ -13,7 +13,9 @@
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/make_shared.hpp>
@ -23,31 +25,61 @@ class SubgraphSolverParameters : public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
SubgraphSolverParameters() : Base() {}
virtual void print(const std::string &s="") const { Base::print(s); }
};
/**
* A linear system solver using subgraph preconditioning conjugate gradient
* This class implements 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.
*
* To use it in nonlinear optimization, please see the following example
*
* LevenbergMarquardtParams parameters;
* parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
* parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
* LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
* Values result = optimizer.optimize();
*
* \nosubgrouping
*/
class SubgraphSolver : public IterativeSolver {
public:
typedef SubgraphSolverParameters Parameters;
protected:
Parameters parameters_;
SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object
public:
/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters);
SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters &parameters);
/* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
/* The same as above, but the A1 is solved before */
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters);
virtual ~SubgraphSolver() {}
virtual VectorValues optimize () ;
protected:
void initialize(const GaussianFactorGraph &jfg);
void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2);
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
splitGraph(const GaussianFactorGraph &gfg) ;
};

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

@ -122,7 +122,7 @@ namespace gtsam {
// conjugate gradient method.
// S: linear system, V: step vector, E: errors
template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters &parameters, bool steepest = false) {
V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters &parameters, bool steepest) {
CGState<S, V, E> state(Ab, x, parameters, steepest);

71
gtsam/linear/iterative.cpp
View File

@ -19,53 +19,60 @@
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h>
#include <iostream>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
void System::print (const string& s) const {
cout << s << endl;
gtsam::print(A_, "A");
gtsam::print(b_, "b");
}
/* ************************************************************************* */
void System::print(const string& s) const {
cout << s << endl;
gtsam::print(A_, "A");
gtsam::print(b_, "b");
}
/* ************************************************************************* */
/* ************************************************************************* */
Vector steepestDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
}
Vector steepestDescent(const System& Ab, const Vector& x,
const ConjugateGradientParameters & parameters) {
return conjugateGradients<System, Vector, Vector>(Ab, x, parameters, true);
}
Vector conjugateGradientDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
}
Vector conjugateGradientDescent(const System& Ab, const Vector& x,
const ConjugateGradientParameters & parameters) {
return conjugateGradients<System, Vector, Vector>(Ab, x, parameters);
}
/* ************************************************************************* */
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) {
System Ab(A, b);
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
}
/* ************************************************************************* */
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
const ConjugateGradientParameters & parameters) {
System Ab(A, b);
return conjugateGradients<System, Vector, Vector>(Ab, x, parameters, true);
}
Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) {
System Ab(A, b);
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
}
Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
const Vector& x, const ConjugateGradientParameters & parameters) {
System Ab(A, b);
return conjugateGradients<System, Vector, Vector>(Ab, x, parameters);
}
/* ************************************************************************* */
VectorValues steepestDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters, true);
}
/* ************************************************************************* */
VectorValues steepestDescent(const GaussianFactorGraph& fg,
const VectorValues& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors>(
fg, x, parameters, true);
}
VectorValues conjugateGradientDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters);
}
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorValues& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors>(
fg, x, parameters);
}
/* ************************************************************************* */
/* ************************************************************************* */
} // namespace gtsam

13
gtsam/linear/iterative.h
View File

@ -31,12 +31,11 @@ namespace gtsam {
* "Vector" class E needs dot(v,v)
* @param Ab, the "system" that needs to be solved, examples below
* @param x is the initial estimate
* @param epsilon determines the convergence criterion: norm(g)<epsilon*norm(g0)
* @param maxIterations, if 0 will be set to |x|
* @param steepest flag, if true does steepest descent, not CG
* */
template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, size_t maxIterations, bool steepest = false);
template<class S, class V, class E>
V conjugateGradients(const S& Ab, V x,
const ConjugateGradientParameters &parameters, bool steepest = false);
/**
* Helper class encapsulating the combined system |Ax-b_|^2
@ -127,11 +126,9 @@ namespace gtsam {
const Vector& x,
const ConjugateGradientParameters & parameters);
class GaussianFactorGraph;
/**
* Method of steepest gradients, Gaussian Factor Graph version
* */
*/
VectorValues steepestDescent(
const GaussianFactorGraph& fg,
const VectorValues& x,
@ -139,7 +136,7 @@ namespace gtsam {
/**
* Method of conjugate gradients (CG), Gaussian Factor Graph version
* */
*/
VectorValues conjugateGradientDescent(
const GaussianFactorGraph& fg,
const VectorValues& x,

21
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -410,7 +410,7 @@ TEST(GaussianFactor, eliminateFrontals)
factors.push_back(factor4);
// extract the dense matrix for the graph
Matrix actualDense = factors.denseJacobian();
Matrix actualDense = factors.augmentedJacobian();
EXPECT(assert_equal(2.0 * Ab, actualDense));
// Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians
@ -619,7 +619,7 @@ TEST(GaussianFactorGraph, sparseJacobian) {
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, denseHessian) {
TEST(GaussianFactorGraph, matrices) {
// Create factor graph:
// x1 x2 x3 x4 x5 b
// 1 2 3 0 0 4
@ -639,9 +639,24 @@ TEST(GaussianFactorGraph, denseHessian) {
9,10, 0,11,12,13,
0, 0, 0,14,15,16;
Matrix expectedJacobian = jacobian;
Matrix expectedHessian = jacobian.transpose() * jacobian;
Matrix actualHessian = gfg.denseHessian();
Matrix expectedA = jacobian.leftCols(jacobian.cols()-1);
Vector expectedb = jacobian.col(jacobian.cols()-1);
Matrix expectedL = expectedA.transpose() * expectedA;
Vector expectedeta = expectedA.transpose() * expectedb;
Matrix actualJacobian = gfg.augmentedJacobian();
Matrix actualHessian = gfg.augmentedHessian();
Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian();
Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian();
EXPECT(assert_equal(expectedJacobian, actualJacobian));
EXPECT(assert_equal(expectedHessian, actualHessian));
EXPECT(assert_equal(expectedA, actualA));
EXPECT(assert_equal(expectedb, actualb));
EXPECT(assert_equal(expectedL, actualL));
EXPECT(assert_equal(expectedeta, actualeta));
}
/* ************************************************************************* */

92
gtsam/linear/tests/testJacobianFactorGraph.cpp Normal file
View File

@ -0,0 +1,92 @@
/* ----------------------------------------------------------------------------
* 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 testJacobianFactorGraph.cpp
* @brief Unit tests for GaussianFactorGraph
* @author Yong Dian Jian
**/
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
///* ************************************************************************* */
//TEST( GaussianFactorGraph, gradient )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// // Construct expected gradient
// VectorValues expected;
//
// // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
// expected.insert(L(1),Vector_(2, 5.0,-12.5));
// expected.insert(X(1),Vector_(2, 30.0, 5.0));
// expected.insert(X(2),Vector_(2,-25.0, 17.5));
//
// // Check the gradient at delta=0
// VectorValues zero = createZeroDelta();
// VectorValues actual = fg.gradient(zero);
// EXPECT(assert_equal(expected,actual));
//
// // Check it numerically for good measure
// Vector numerical_g = numericalGradient<VectorValues>(error,zero,0.001);
// EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g));
//
// // Check the gradient at the solution (should be zero)
// Ordering ord;
// ord += X(2),L(1),X(1);
// GaussianFactorGraph fg2 = createGaussianFactorGraph();
// VectorValues solution = fg2.optimize(ord); // destructive
// VectorValues actual2 = fg.gradient(solution);
// EXPECT(assert_equal(zero,actual2));
//}
///* ************************************************************************* */
//TEST( GaussianFactorGraph, transposeMultiplication )
//{
// // create an ordering
// Ordering ord; ord += X(2),L(1),X(1);
//
// GaussianFactorGraph A = createGaussianFactorGraph(ord);
// Errors e;
// e += Vector_(2, 0.0, 0.0);
// e += Vector_(2,15.0, 0.0);
// e += Vector_(2, 0.0,-5.0);
// e += Vector_(2,-7.5,-5.0);
//
// VectorValues expected = createZeroDelta(ord), actual = A ^ e;
// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0);
// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0);
// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0);
// EXPECT(assert_equal(expected,actual));
//}
///* ************************************************************************* */
//TEST( GaussianFactorGraph, rhs )
//{
// // create an ordering
// Ordering ord; ord += X(2),L(1),X(1);
//
// GaussianFactorGraph Ab = createGaussianFactorGraph(ord);
// Errors expected = createZeroDelta(ord), actual = Ab.rhs();
// expected.push_back(Vector_(2,-1.0,-1.0));
// expected.push_back(Vector_(2, 2.0,-1.0));
// expected.push_back(Vector_(2, 0.0, 1.0));
// expected.push_back(Vector_(2,-1.0, 1.5));
// EXPECT(assert_equal(expected,actual));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

2
gtsam/nonlinear/ISAM2.cpp
View File

@ -27,7 +27,7 @@ using namespace boost::assign;
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>

3
gtsam/nonlinear/ISAM2.h
View File

@ -598,6 +598,9 @@ public:
const ISAM2Params& params() const { return params_; }
/** prints out clique statistics */
void printStats() const { getCliqueData().getStats().print(); }
//@}
private:

2
gtsam/nonlinear/Marginals.cpp
View File

@ -152,7 +152,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
}
// Get information matrix
Matrix augmentedInfo = jointFG.denseHessian();
Matrix augmentedInfo = jointFG.augmentedHessian();
Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1);
return JointMarginal(info, dims, variableConversion);

2
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -8,7 +8,7 @@
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <cmath>

17
gtsam/nonlinear/Ordering.cpp
View File

@ -44,10 +44,23 @@ void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const
cout << str;
// Print ordering in index order
Ordering::InvertedMap inverted = this->invert();
cout << "key (zero-based order)\n";
// Print the ordering with varsPerLine ordering entries printed on each line,
// for compactness.
static const size_t varsPerLine = 10;
bool endedOnNewline = false;
BOOST_FOREACH(const Ordering::InvertedMap::value_type& index_key, inverted) {
cout << keyFormatter(index_key.second) << " (" << index_key.first << ")\n";
if(index_key.first % varsPerLine != 0)
cout << ", ";
cout << index_key.first << ":" << keyFormatter(index_key.second);
if(index_key.first % varsPerLine == varsPerLine - 1) {
cout << "\n";
endedOnNewline = true;
} else {
endedOnNewline = false;
}
}
if(!endedOnNewline)
cout << "\n";
cout.flush();
}

11
gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp
View File

@ -8,7 +8,6 @@
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
@ -16,6 +15,10 @@
namespace gtsam {
void SuccessiveLinearizationParams::setIterativeParams(const SubgraphSolverParameters &params) {
iterativeParams = boost::make_shared<SubgraphSolverParameters>(params);
}
void SuccessiveLinearizationParams::print(const std::string& str) const {
NonlinearOptimizerParams::print(str);
switch ( linearSolverType ) {
@ -60,11 +63,7 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ
}
else if ( params.isCG() ) {
if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ...");
if ( boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params.iterativeParams)) {
SimpleSPCGSolver solver (gfg, *boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params.iterativeParams));
delta = solver.optimize();
}
else if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams));
delta = solver.optimize();
}

3
gtsam/nonlinear/SuccessiveLinearizationOptimizer.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/SubgraphSolver.h>
namespace gtsam {
@ -74,6 +74,7 @@ public:
std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); }
void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); }
void setIterativeParams(const SubgraphSolverParameters &params);
void setOrdering(const Ordering& ordering) { this->ordering = ordering; }
private:

2
gtsam/slam/tests/testDataset.cpp
View File

@ -26,7 +26,7 @@ using namespace std;
using namespace gtsam;
TEST(dataSet, findExampleDataFile) {
const string expected_end = "gtsam/examples/Data/example.graph";
const string expected_end = "examples/Data/example.graph";
const string actual = findExampleDataFile("example");
string actual_end = actual.substr(actual.size() - expected_end.size(), expected_end.size());
boost::replace_all(actual_end, "\\", "/"); // Convert directory separators to forward-slash

43
gtsam_unstable/base/Dummy.cpp Normal file
View File

@ -0,0 +1,43 @@
/* ----------------------------------------------------------------------------
* 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 Dummy.h
* @brief Dummy class for testing MATLAB memory allocation
* @author Andrew Melim
* @author Frank Dellaert
* @date June 14, 2012
*/
#include <gtsam_unstable/base/Dummy.h>
#include <iostream>
namespace gtsam {
static size_t gDummyCount = 0;
Dummy::Dummy():id(++gDummyCount) {
std::cout << "Dummy constructor " << id << std::endl;
}
Dummy::~Dummy() {
std::cout << "Dummy destructor " << id << std::endl;
}
void Dummy::print(const std::string& s) const {
std::cout << s << "Dummy " << id << std::endl;
}
unsigned char Dummy::dummyTwoVar(unsigned char a) const {
return a;
}
}

22
gtsam_unstable/base/Dummy.h
View File

@ -17,26 +17,16 @@
* @date June 14, 2012
*/
namespace gtsam {
#include <string>
static size_t gDummyCount;
namespace gtsam {
struct Dummy {
size_t id;
Dummy():id(++gDummyCount) {
std::cout << "Dummy constructor " << id << std::endl;
}
~Dummy() {
std::cout << "Dummy destructor " << id << std::endl;
}
void print(const std::string& s="") const {
std::cout << s << "Dummy " << id << std::endl;
}
unsigned char dummyTwoVar(unsigned char a) const {
return a;
}
Dummy();
~Dummy();
void print(const std::string& s="") const ;
unsigned char dummyTwoVar(unsigned char a) const ;
};
} // namespace gtsam

11
gtsam_unstable/nonlinear/LinearContainerFactor.cpp
View File

@ -38,6 +38,15 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza
}
}
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
const boost::optional<Values>& linearizationPoint)
: factor_(factor), linearizationPoint_(linearizationPoint) {
// Extract keys stashed in linear factor
BOOST_FOREACH(const Index& idx, factor_->keys())
keys_.push_back(idx);
}
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(
const JacobianFactor& factor, const Ordering& ordering,
@ -205,7 +214,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& orderin
/* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::negate() const {
GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place
return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor));
return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor,linearizationPoint_));
}
/* ************************************************************************* */

6
gtsam_unstable/nonlinear/LinearContainerFactor.h
View File

@ -23,6 +23,10 @@ protected:
GaussianFactor::shared_ptr factor_;
boost::optional<Values> linearizationPoint_;
/** direct copy constructor */
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
const boost::optional<Values>& linearizationPoint);
public:
/** Primary constructor: store a linear factor and decode the ordering */
@ -105,7 +109,7 @@ public:
* Clones the underlying linear factor
*/
NonlinearFactor::shared_ptr clone() const {
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_));
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
}
// casting syntactic sugar

4
gtsam_unstable/slam/PoseRotationPrior.h
View File

@ -67,9 +67,9 @@ public:
if (H) {
*H = gtsam::zeros(rDim, xDim);
if (pose_traits::isRotFirst<Pose>())
(*H).leftCols(rDim) = eye(rDim);
(*H).leftCols(rDim).setIdentity(rDim, rDim);
else
(*H).rightCols(rDim) = eye(rDim);
(*H).rightCols(rDim).setIdentity(rDim, rDim);
}
return Rotation::Logmap(newR) - Rotation::Logmap(measured_);

2
matlab/+gtsam/VisualISAMInitialize.m
View File

@ -50,7 +50,7 @@ for i=1:2
initialEstimates.insert(jj, truth.points{j});
end
if options.pointPriors % add point priors
newFactors.add(priorFactorPoint3(jj, truth.points{j}, noiseModels.point));
newFactors.add(PriorFactorPoint3(jj, truth.points{j}, noiseModels.point));
end
end
end

9
matlab/+gtsam/VisualISAMPlot.m
View File

@ -39,11 +39,12 @@ drawnow
%% do various optional things
if options.saveFigures
fig2 = figure('visible','off');
newax = copyobj(h,fig2);
colormap(fig2,'hot');
figToSave = figure('visible','off');
newax = copyobj(h,figToSave);
colormap(figToSave,'hot');
set(newax, 'units', 'normalized', 'position', [0.13 0.11 0.775 0.815]);
print(fig2,'-dpng',sprintf('VisualiSAM%03d.png',M));
print(figToSave,'-dpng',sprintf('VisualiSAM%03d.png',M));
axes(h);
end
if options.saveDotFiles

7
matlab/CMakeLists.txt
View File

@ -2,17 +2,16 @@
# Tests
message(STATUS "Installing Matlab Toolbox Tests")
install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m")
install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE)
# Examples
message(STATUS "Installing Matlab Toolbox Examples")
# Matlab files: *.m and *.fig
install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m")
install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.fig")
install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN "*.fig" PATTERN ".svn" EXCLUDE)
# Utilities
message(STATUS "Installing Matlab Toolbox Utilities")
install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m")
install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE)
install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}")
message(STATUS "Installing Matlab Toolbox Examples (Data)")

2
matlab/gtsam_examples/PlanarSLAMExample_graph.m
View File

@ -15,7 +15,7 @@ clear
import gtsam.*
%% Find data file
datafile = '/Users/dellaert/borg/gtsam/examples/Data/example.graph';
datafile = findExampleDataFile('example.graph');
%% Initialize graph, initial estimate, and odometry noise
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);

15
matlab/gtsam_examples/Pose2SLAMwSPCG.m
View File

@ -51,7 +51,14 @@ initialEstimate.insert(4, Pose2(4.0, 2.0, pi ));
initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2));
initialEstimate.print(sprintf('\nInitial estimate:\n'));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
optimizer = DoglegOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely();
result.print(sprintf('\nFinal result:\n'));
%% Optimize using Levenberg-Marquardt optimization with SubgraphSolver
params = gtsam.LevenbergMarquardtParams;
subgraphParams = gtsam.SubgraphSolverParameters;
params.setLinearSolverType('CONJUGATE_GRADIENT');
params.setIterativeParams(subgraphParams);
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize();
result.print(sprintf('\nFinal result:\n'));

BIN
matlab/gtsam_examples/VisualISAM_gui.fig
View File

Binary file not shown.

2
matlab/gtsam_examples/VisualISAM_gui.m
View File

@ -82,7 +82,7 @@ options.showImages = get(handles.showImagesCB,'Value');
options.hardConstraint = get(handles.hardConstraintCB,'Value');
options.pointPriors = get(handles.pointPriorsCB,'Value');
options.batchInitialization = get(handles.batchInitCB,'Value');
options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String'));
%options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String'));
options.alwaysRelinearize = get(handles.alwaysRelinearizeCB,'Value');
% Display Options

BIN
matlab/gtsam_examples/gtsamExamples.fig
View File

Binary file not shown.

9
matlab/gtsam_examples/gtsamExamples.m
View File

@ -22,7 +22,7 @@ function varargout = gtsamExamples(varargin)
% Edit the above text to modify the response to help gtsamExamples
% Last Modified by GUIDE v2.5 24-Jul-2012 10:18:05
% Last Modified by GUIDE v2.5 03-Sep-2012 13:34:13
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
@ -142,6 +142,13 @@ function PlanarSLAMSampling_Callback(hObject, eventdata, handles)
axes(handles.axes3);
PlanarSLAMExample_sampling
% --- Executes on button press in PlanarSLAMGraph.
function PlanarSLAMGraph_Callback(hObject, eventdata, handles)
axes(handles.axes3);
echo on
PlanarSLAMExample_graph
echo off
% --- Executes on button press in SFM.
function SFM_Callback(hObject, eventdata, handles)
axes(handles.axes3);

43
package_scripts/toolbox_package_unix.sh Normal file
View File

@ -0,0 +1,43 @@
#!/bin/sh
# Detect platform
os=`uname -s`
arch=`uname -m`
if [ "$os" = "Linux" -a "$arch" = "x86_64" ]; then
platform=lin64
elif [ "$os" = "Linux" -a "$arch" = "i686" ]; then
platform=lin32
else
echo "Unrecognized platform"
exit 1
fi
echo "Platform is ${platform}"
# Check for empty directory
if [ ! -z "`ls`" ]; then
echo "Please run this script from an empty build directory"
exit 1
fi
# Check for boost
if [ -z "$1" -o -z "$2" ]; then
echo "Usage: $0 BOOSTTREE MEX"
echo "BOOSTTREE should be a boost source tree compiled with toolbox_build_boost."
echo "MEX should be the full path of the mex compiler"
exit 1
fi
# Run cmake
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="$PWD/stage" -DBoost_NO_SYSTEM_PATHS:bool=true -DBoost_USE_STATIC_LIBS:bool=true -DBOOST_ROOT="$1" -DGTSAM_BUILD_UNSTABLE:bool=false -DGTSAM_DISABLE_EXAMPLES_ON_INSTALL:bool=true -DGTSAM_DISABLE_TESTS_ON_INSTALL:bool=true -DGTSAM_MEX_BUILD_STATIC_MODULE:bool=true -DMEX_COMMAND="$2" ..
if [ ! $? ]; then
echo "CMake failed"
exit 1
fi
# Compile
make install
# Create package
tar czf gtsam-toolbox-2.1.0-$platform.tgz -C stage/borg toolbox

25
tests/smallExample.cpp
View File

@ -138,9 +138,9 @@ namespace example {
}
/* ************************************************************************* */
JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
// Create empty graph
JacobianFactorGraph fg;
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
@ -273,7 +273,7 @@ namespace example {
}
/* ************************************************************************* */
JacobianFactorGraph createSimpleConstraintGraph() {
GaussianFactorGraph createSimpleConstraintGraph() {
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
@ -293,7 +293,7 @@ namespace example {
constraintModel));
// construct the graph
JacobianFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
@ -310,7 +310,7 @@ namespace example {
}
/* ************************************************************************* */
JacobianFactorGraph createSingleConstraintGraph() {
GaussianFactorGraph createSingleConstraintGraph() {
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
@ -335,7 +335,7 @@ namespace example {
constraintModel));
// construct the graph
JacobianFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
@ -351,7 +351,7 @@ namespace example {
}
/* ************************************************************************* */
JacobianFactorGraph createMultiConstraintGraph() {
GaussianFactorGraph createMultiConstraintGraph() {
// unary factor 1
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
@ -396,7 +396,7 @@ namespace example {
constraintModel));
// construct the graph
JacobianFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(lf1);
fg.push_back(lc1);
fg.push_back(lc2);
@ -458,7 +458,8 @@ namespace example {
xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
// linearize around zero
return boost::make_tuple(*nlfg.linearize(zeros, ordering), xtrue);
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
return boost::make_tuple(*gfg, xtrue);
}
/* ************************************************************************* */
@ -471,9 +472,9 @@ namespace example {
}
/* ************************************************************************* */
pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(size_t N,
const JacobianFactorGraph& original) {
JacobianFactorGraph T, C;
pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original) {
GaussianFactorGraph T, C;
// Add the x11 constraint to the tree
T.push_back(original[0]);

13
tests/smallExample.h
View File

@ -23,7 +23,6 @@
#include <tests/simulated2D.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <boost/tuple/tuple.hpp>
namespace gtsam {
@ -66,7 +65,7 @@ namespace gtsam {
* create a linear factor graph
* The non-linear graph above evaluated at NoisyValues
*/
JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering);
GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering);
/**
* create small Chordal Bayes Net x <- y
@ -100,21 +99,21 @@ namespace gtsam {
* Creates a simple constrained graph with one linear factor and
* one binary equality constraint that sets x = y
*/
JacobianFactorGraph createSimpleConstraintGraph();
GaussianFactorGraph createSimpleConstraintGraph();
VectorValues createSimpleConstraintValues();
/**
* Creates a simple constrained graph with one linear factor and
* one binary constraint.
*/
JacobianFactorGraph createSingleConstraintGraph();
GaussianFactorGraph createSingleConstraintGraph();
VectorValues createSingleConstraintValues();
/**
* Creates a constrained graph with a linear factor and two
* binary constraints that share a node
*/
JacobianFactorGraph createMultiConstraintGraph();
GaussianFactorGraph createMultiConstraintGraph();
VectorValues createMultiConstraintValues();
/* ******************************************************* */
@ -147,8 +146,8 @@ namespace gtsam {
* |
* -x11-x21-x31
*/
std::pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(
size_t N, const JacobianFactorGraph& original);
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
size_t N, const GaussianFactorGraph& original);
} // example
} // gtsam

6
tests/testDoglegOptimizer.cpp
View File

@ -96,7 +96,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
gradientValues.vector() = gradient;
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).denseHessian();
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
LONGS_EQUAL(11, augmentedHessian.cols());
VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
@ -200,7 +200,7 @@ TEST(DoglegOptimizer, BT_BN_equivalency) {
GaussianFactorGraph expected(gbn);
GaussianFactorGraph actual(bt);
EXPECT(assert_equal(expected.denseHessian(), actual.denseHessian()));
EXPECT(assert_equal(expected.augmentedHessian(), actual.augmentedHessian()));
}
/* ************************************************************************* */
@ -276,7 +276,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
gradientValues.vector() = gradient;
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(bt).denseHessian();
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
LONGS_EQUAL(11, augmentedHessian.cols());
VectorValues denseMatrixGradient = *allocateVectorValues(bt);
denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);

193
tests/testGaussianFactor.cpp
View File

@ -57,42 +57,16 @@ TEST( GaussianFactor, linearFactor )
JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2));
// create a small linear factor graph
FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
// get the factor kf2 from the factor graph
JacobianFactor::shared_ptr lf = fg[1];
JacobianFactor::shared_ptr lf =
boost::dynamic_pointer_cast<JacobianFactor>(fg[1]);
// check if the two factors are the same
EXPECT(assert_equal(expected,*lf));
}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactor, keys )
//{
// // get the factor kf2 from the small linear factor graph
// Ordering ordering; ordering += kx1,kx2,kl1;
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianFactor::shared_ptr lf = fg[1];
// list<Symbol> expected;
// expected.push_back(kx1);
// expected.push_back(kx2);
// EXPECT(lf->keys() == expected);
//}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactor, dimensions )
//{
// // get the factor kf2 from the small linear factor graph
// Ordering ordering; ordering += kx1,kx2,kl1;
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
//
// // Check a single factor
// Dimensions expected;
// insert(expected)(kx1, 2)(kx2, 2);
// Dimensions actual = fg[1]->dimensions();
// EXPECT(expected==actual);
//}
/* ************************************************************************* */
TEST( GaussianFactor, getDim )
{
@ -110,62 +84,6 @@ TEST( GaussianFactor, getDim )
EXPECT_LONGS_EQUAL(expected, actual);
}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactor, combine )
//{
// // create a small linear factor graph
// Ordering ordering; ordering += kx1,kx2,kl1;
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
//
// // get two factors from it and insert the factors into a vector
// vector<GaussianFactor::shared_ptr> lfg;
// lfg.push_back(fg[4 - 1]);
// lfg.push_back(fg[2 - 1]);
//
// // combine in a factor
// GaussianFactor combined(lfg);
//
// // sigmas
// double sigma2 = 0.1;
// double sigma4 = 0.2;
// Vector sigmas = Vector_(4, sigma4, sigma4, sigma2, sigma2);
//
// // the expected combined linear factor
// Matrix Ax2 = Matrix_(4, 2, // x2
// -5., 0.,
// +0., -5.,
// 10., 0.,
// +0., 10.);
//
// Matrix Al1 = Matrix_(4, 2, // l1
// 5., 0.,
// 0., 5.,
// 0., 0.,
// 0., 0.);
//
// Matrix Ax1 = Matrix_(4, 2, // x1
// 0.00, 0., // f4
// 0.00, 0., // f4
// -10., 0., // f2
// 0.00, -10. // f2
// );
//
// // the RHS
// Vector b2(4);
// b2(0) = -1.0;
// b2(1) = 1.5;
// b2(2) = 2.0;
// b2(3) = -1.0;
//
// // use general constructor for making arbitrary factors
// vector<pair<Symbol, Matrix> > meas;
// meas.push_back(make_pair(kx2, Ax2));
// meas.push_back(make_pair(kl1, Al1));
// meas.push_back(make_pair(kx1, Ax1));
// GaussianFactor expected(meas, b2, noiseModel::Diagonal::Sigmas(ones(4)));
// EXPECT(assert_equal(expected,combined));
//}
/* ************************************************************************* */
TEST( GaussianFactor, error )
{
@ -186,54 +104,13 @@ TEST( GaussianFactor, error )
DOUBLES_EQUAL( 1.0, actual, 0.00000001 );
}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactor, eliminate )
//{
// // create a small linear factor graph
// Ordering ordering; ordering += kx1,kx2,kl1;
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
//
// // get two factors from it and insert the factors into a vector
// vector<GaussianFactor::shared_ptr> lfg;
// lfg.push_back(fg[4 - 1]);
// lfg.push_back(fg[2 - 1]);
//
// // combine in a factor
// GaussianFactor combined(lfg);
//
// // eliminate the combined factor
// GaussianConditional::shared_ptr actualCG;
// GaussianFactor::shared_ptr actualLF;
// boost::tie(actualCG,actualLF) = combined.eliminate(kx2);
//
// // create expected Conditional Gaussian
// Matrix I = eye(2)*sqrt(125.0);
// Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I;
// Vector d = I*Vector_(2,0.2,-0.14);
//
// // Check the conditional Gaussian
// GaussianConditional
// expectedCG(kx2, d, R11, kl1, S12, kx1, S13, repeat(2, 1.0));
//
// // the expected linear factor
// I = eye(2)/0.2236;
// Matrix Bl1 = I, Bx1 = -I;
// Vector b1 = I*Vector_(2,0.0,0.2);
//
// GaussianFactor expectedLF(kl1, Bl1, kx1, Bx1, b1, repeat(2,1.0));
//
// // check if the result matches
// EXPECT(assert_equal(expectedCG,*actualCG,1e-3));
// EXPECT(assert_equal(expectedLF,*actualLF,1e-3));
//}
/* ************************************************************************* */
TEST( GaussianFactor, matrix )
{
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
// create a small linear factor graph
Ordering ordering; ordering += kx1,kx2,kl1;
FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
// get the factor kf2 from the factor graph
//GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version
@ -282,7 +159,7 @@ TEST( GaussianFactor, matrix_aug )
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
// create a small linear factor graph
Ordering ordering; ordering += kx1,kx2,kl1;
FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
// get the factor kf2 from the factor graph
//GaussianFactor::shared_ptr lf = fg[1];
@ -328,66 +205,6 @@ void print(const list<T>& i) {
cout << endl;
}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactor, sparse )
//{
// // create a small linear factor graph
// Ordering ordering; ordering += kx1,kx2,kl1;
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
//
// // get the factor kf2 from the factor graph
// GaussianFactor::shared_ptr lf = fg[1];
//
// // render with a given ordering
// Ordering ord;
// ord += kx1,kx2;
//
// list<int> i,j;
// list<double> s;
// boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord));
//
// list<int> i1,j1;
// i1 += 1,2,1,2;
// j1 += 1,2,3,4;
//
// list<double> s1;
// s1 += -10,-10,10,10;
//
// EXPECT(i==i1);
// EXPECT(j==j1);
// EXPECT(s==s1);
//}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactor, sparse2 )
//{
// // create a small linear factor graph
// Ordering ordering; ordering += kx1,kx2,kl1;
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
//
// // get the factor kf2 from the factor graph
// GaussianFactor::shared_ptr lf = fg[1];
//
// // render with a given ordering
// Ordering ord;
// ord += kx2,kl1,kx1;
//
// list<int> i,j;
// list<double> s;
// boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord));
//
// list<int> i1,j1;
// i1 += 1,2,1,2;
// j1 += 5,6,1,2;
//
// list<double> s1;
// s1 += -10,-10,10,10;
//
// EXPECT(i==i1);
// EXPECT(j==j1);
// EXPECT(s==s1);
//}
/* ************************************************************************* */
TEST( GaussianFactor, size )
{

554
tests/testGaussianFactorGraphB.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Matrix.h>
@ -56,143 +56,17 @@ TEST( GaussianFactorGraph, equals ) {
}
/* ************************************************************************* */
//TEST( GaussianFactorGraph, error ) {
// Ordering ordering; ordering += X(1),X(2),L(1);
// FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering);
// VectorValues cfg = createZeroDelta(ordering);
//
// // note the error is the same as in testNonlinearFactorGraph as a
// // zero delta config in the linear graph is equivalent to noisy in
// // non-linear, which is really linear under the hood
// double actual = fg.error(cfg);
// DOUBLES_EQUAL( 5.625, actual, 1e-9 );
//}
TEST( GaussianFactorGraph, error ) {
Ordering ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
VectorValues cfg = createZeroDelta(ordering);
/* ************************************************************************* */
/* unit test for find seperator */
/* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, find_separator )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// set<Symbol> separator = fg.find_separator(X(2));
// set<Symbol> expected;
// expected.insert(X(1));
// expected.insert(L(1));
//
// EXPECT(separator.size()==expected.size());
// set<Symbol>::iterator it1 = separator.begin(), it2 = expected.begin();
// for(; it1!=separator.end(); it1++, it2++)
// EXPECT(*it1 == *it2);
//}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, combine_factors_x1 )
//{
// // create a small example for a linear factor graph
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// // combine all factors
// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1));
//
// // the expected linear factor
// Matrix Al1 = Matrix_(6,2,
// 0., 0.,
// 0., 0.,
// 0., 0.,
// 0., 0.,
// 5., 0.,
// 0., 5.
// );
//
// Matrix Ax1 = Matrix_(6,2,
// 10., 0.,
// 0., 10.,
// -10., 0.,
// 0.,-10.,
// -5., 0.,
// 0.,-5.
// );
//
// Matrix Ax2 = Matrix_(6,2,
// 0., 0.,
// 0., 0.,
// 10., 0.,
// 0., 10.,
// 0., 0.,
// 0., 0.
// );
//
// // the expected RHS vector
// Vector b(6);
// b(0) = -1;
// b(1) = -1;
// b(2) = 2;
// b(3) = -1;
// b(4) = 0;
// b(5) = 1;
//
// vector<pair<Symbol, Matrix> > meas;
// meas.push_back(make_pair(L(1), Al1));
// meas.push_back(make_pair(X(1), Ax1));
// meas.push_back(make_pair(X(2), Ax2));
// GaussianFactor expected(meas, b, ones(6));
// //GaussianFactor expected(L(1), Al1, X(1), Ax1, X(2), Ax2, b);
//
// // check if the two factors are the same
// EXPECT(assert_equal(expected,*actual));
//}
//
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, combine_factors_x2 )
//{
// // create a small example for a linear factor graph
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// // combine all factors
// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(2));
//
// // the expected linear factor
// Matrix Al1 = Matrix_(4,2,
// // l1
// 0., 0.,
// 0., 0.,
// 5., 0.,
// 0., 5.
// );
//
// Matrix Ax1 = Matrix_(4,2,
// // x1
// -10., 0., // f2
// 0.,-10., // f2
// 0., 0., // f4
// 0., 0. // f4
// );
//
// Matrix Ax2 = Matrix_(4,2,
// // x2
// 10., 0.,
// 0., 10.,
// -5., 0.,
// 0.,-5.
// );
//
// // the expected RHS vector
// Vector b(4);
// b(0) = 2;
// b(1) = -1;
// b(2) = -1;
// b(3) = 1.5;
//
// vector<pair<Symbol, Matrix> > meas;
// meas.push_back(make_pair(L(1), Al1));
// meas.push_back(make_pair(X(1), Ax1));
// meas.push_back(make_pair(X(2), Ax2));
// GaussianFactor expected(meas, b, ones(4));
//
// // check if the two factors are the same
// EXPECT(assert_equal(expected,*actual));
//}
// note the error is the same as in testNonlinearFactorGraph as a
// zero delta config in the linear graph is equivalent to noisy in
// non-linear, which is really linear under the hood
double actual = fg.error(cfg);
DOUBLES_EQUAL( 5.625, actual, 1e-9 );
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x1 )
@ -336,47 +210,6 @@ TEST( GaussianFactorGraph, eliminateAll )
EXPECT(assert_equal(expected,actualQR,tol));
}
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateAll_fast )
//{
// // create expected Chordal bayes Net
// Matrix I = eye(2);
//
// Vector d1 = Vector_(2, -0.1,-0.1);
// GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1);
//
// double sig1 = 0.149071;
// Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
// push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2);
//
// double sig2 = 0.0894427;
// Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
// push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3);
//
// // Check one ordering
// GaussianFactorGraph fg1 = createGaussianFactorGraph();
// Ordering ordering;
// ordering += X(2),L(1),X(1);
// GaussianBayesNet actual = fg1.eliminate(ordering, false);
// EXPECT(assert_equal(expected,actual,tol));
//}
///* ************************************************************************* */
//TEST( GaussianFactorGraph, add_priors )
//{
// Ordering ordering; ordering += L(1),X(1),X(2);
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
// GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
// Matrix A = eye(2);
// Vector b = zero(2);
// SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2,3.0);
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma)));
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma)));
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma)));
// EXPECT(assert_equal(expected,actual));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, copying )
{
@ -397,46 +230,6 @@ TEST( GaussianFactorGraph, copying )
EXPECT(assert_equal(expected,actual));
}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, matrix )
//{
// // render with a given ordering
// Ordering ord;
// ord += X(2),L(1),X(1);
//
// // Create a graph
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
//
// Matrix A; Vector b;
// boost::tie(A,b) = fg.matrix();
//
// Matrix A1 = Matrix_(2*4,3*2,
// +0., 0., 0., 0., 10., 0., // unary factor on x1 (prior)
// +0., 0., 0., 0., 0., 10.,
// 10., 0., 0., 0.,-10., 0., // binary factor on x2,x1 (odometry)
// +0., 10., 0., 0., 0.,-10.,
// +0., 0., 5., 0., -5., 0., // binary factor on l1,x1 (z1)
// +0., 0., 0., 5., 0., -5.,
// -5., 0., 5., 0., 0., 0., // binary factor on x2,l1 (z2)
// +0., -5., 0., 5., 0., 0.
// );
// Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5);
//
// EQUALITY(A,A1);
// EXPECT(b==b1);
//}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, sizeOfA )
//{
// // create a small linear factor graph
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// pair<size_t, size_t> mn = fg.sizeOfA();
// EXPECT(8 == mn.first);
// EXPECT(6 == mn.second);
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
{
@ -451,11 +244,6 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
GaussianFactorGraph fg2(CBN);
GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
EXPECT(assert_equal(CBN,CBN2));
// Base FactorGraph only
// FactorGraph<GaussianFactor> fg3(CBN);
// GaussianBayesNet CBN3 = gtsam::eliminate<GaussianFactor,GaussianConditional>(fg3,ord);
// EXPECT(assert_equal(CBN,CBN3));
}
/* ************************************************************************* */
@ -469,16 +257,6 @@ TEST( GaussianFactorGraph, getOrdering)
EXPECT(assert_equal(expected,actual));
}
// SL-FIX TEST( GaussianFactorGraph, getOrdering2)
//{
// Ordering expected;
// expected += L(1),X(1);
// GaussianFactorGraph fg = createGaussianFactorGraph();
// set<Symbol> interested; interested += L(1),X(1);
// Ordering actual = fg.getOrdering(interested);
// EXPECT(assert_equal(expected,actual));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, optimize_Cholesky )
{
@ -515,24 +293,6 @@ TEST( GaussianFactorGraph, optimize_QR )
EXPECT(assert_equal(expected,actual));
}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas )
//{
// // create an ordering
// Ordering ord; ord += X(2),L(1),X(1);
//
// // create a graph
// GaussianFactorGraph fg = createGaussianFactorGraph(ord);
//
// // optimize the graph
// VectorValues actual = fg.optimizeMultiFrontals(ord);
//
// // verify
// VectorValues expected = createCorrectDelta();
//
// EXPECT(assert_equal(expected,actual));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, combine)
{
@ -585,48 +345,6 @@ void print(vector<int> v) {
cout << endl;
}
///* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, factor_lookup)
//{
// // create a test graph
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// // ask for all factor indices connected to x1
// list<size_t> x1_factors = fg.factors(X(1));
// size_t x1_indices[] = { 0, 1, 2 };
// list<size_t> x1_expected(x1_indices, x1_indices + 3);
// EXPECT(x1_factors==x1_expected);
//
// // ask for all factor indices connected to x2
// list<size_t> x2_factors = fg.factors(X(2));
// size_t x2_indices[] = { 1, 3 };
// list<size_t> x2_expected(x2_indices, x2_indices + 2);
// EXPECT(x2_factors==x2_expected);
//}
///* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, findAndRemoveFactors )
//{
// // create the graph
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// // We expect to remove these three factors: 0, 1, 2
// GaussianFactor::shared_ptr f0 = fg[0];
// GaussianFactor::shared_ptr f1 = fg[1];
// GaussianFactor::shared_ptr f2 = fg[2];
//
// // call the function
// vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors(X(1));
//
// // Check the factors
// EXPECT(f0==factors[0]);
// EXPECT(f1==factors[1]);
// EXPECT(f2==factors[2]);
//
// // EXPECT if the factors are deleted from the factor graph
// LONGS_EQUAL(1,fg.nrFactors());
//}
/* ************************************************************************* */
TEST(GaussianFactorGraph, createSmoother)
{
@ -636,35 +354,6 @@ TEST(GaussianFactorGraph, createSmoother)
LONGS_EQUAL(5,fg2.size());
}
///* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, variables )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
// Dimensions expected;
// insert(expected)(L(1), 2)(X(1), 2)(X(2), 2);
// Dimensions actual = fg.dimensions();
// EXPECT(expected==actual);
//}
///* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, keys )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
// Ordering expected;
// expected += L(1),X(1),X(2);
// EXPECT(assert_equal(expected,fg.keys()));
//}
///* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, involves )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
// EXPECT(fg.involves(L(1)));
// EXPECT(fg.involves(X(1)));
// EXPECT(fg.involves(X(2)));
// EXPECT(!fg.involves(X(3)));
//}
/* ************************************************************************* */
double error(const VectorValues& x) {
// create an ordering
@ -674,45 +363,13 @@ double error(const VectorValues& x) {
return fg.error(x);
}
///* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, gradient )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// // Construct expected gradient
// VectorValues expected;
//
// // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
// expected.insert(L(1),Vector_(2, 5.0,-12.5));
// expected.insert(X(1),Vector_(2, 30.0, 5.0));
// expected.insert(X(2),Vector_(2,-25.0, 17.5));
//
// // Check the gradient at delta=0
// VectorValues zero = createZeroDelta();
// VectorValues actual = fg.gradient(zero);
// EXPECT(assert_equal(expected,actual));
//
// // Check it numerically for good measure
// Vector numerical_g = numericalGradient<VectorValues>(error,zero,0.001);
// EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g));
//
// // Check the gradient at the solution (should be zero)
// Ordering ord;
// ord += X(2),L(1),X(1);
// GaussianFactorGraph fg2 = createGaussianFactorGraph();
// VectorValues solution = fg2.optimize(ord); // destructive
// VectorValues actual2 = fg.gradient(solution);
// EXPECT(assert_equal(zero,actual2));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, multiplication )
{
// create an ordering
Ordering ord; ord += X(2),L(1),X(1);
FactorGraph<JacobianFactor> A = createGaussianFactorGraph(ord);
GaussianFactorGraph A = createGaussianFactorGraph(ord);
VectorValues x = createCorrectDelta(ord);
Errors actual = A * x;
Errors expected;
@ -723,41 +380,6 @@ TEST( GaussianFactorGraph, multiplication )
EXPECT(assert_equal(expected,actual));
}
///* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication )
//{
// // create an ordering
// Ordering ord; ord += X(2),L(1),X(1);
//
// GaussianFactorGraph A = createGaussianFactorGraph(ord);
// Errors e;
// e += Vector_(2, 0.0, 0.0);
// e += Vector_(2,15.0, 0.0);
// e += Vector_(2, 0.0,-5.0);
// e += Vector_(2,-7.5,-5.0);
//
// VectorValues expected = createZeroDelta(ord), actual = A ^ e;
// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0);
// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0);
// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0);
// EXPECT(assert_equal(expected,actual));
//}
///* ************************************************************************* */
// SL-NEEDED? TEST( GaussianFactorGraph, rhs )
//{
// // create an ordering
// Ordering ord; ord += X(2),L(1),X(1);
//
// GaussianFactorGraph Ab = createGaussianFactorGraph(ord);
// Errors expected = createZeroDelta(ord), actual = Ab.rhs();
// expected.push_back(Vector_(2,-1.0,-1.0));
// expected.push_back(Vector_(2, 2.0,-1.0));
// expected.push_back(Vector_(2, 0.0, 1.0));
// expected.push_back(Vector_(2,-1.0, 1.5));
// EXPECT(assert_equal(expected,actual));
//}
/* ************************************************************************* */
// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
TEST( GaussianFactorGraph, elimination )
@ -824,22 +446,6 @@ TEST( GaussianFactorGraph, constrained_single )
EXPECT(assert_equal(expected, actual));
}
///* ************************************************************************* */
//SL-FIX TEST( GaussianFactorGraph, constrained_single2 )
//{
// // get a graph with a constraint in it
// GaussianFactorGraph fg = createSingleConstraintGraph();
//
// // eliminate and solve
// Ordering ord;
// ord += "yk, x";
// VectorValues actual = fg.optimize(ord);
//
// // verify
// VectorValues expected = createSingleConstraintValues();
// EXPECT(assert_equal(expected, actual));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, constrained_multi1 )
{
@ -855,68 +461,9 @@ TEST( GaussianFactorGraph, constrained_multi1 )
EXPECT(assert_equal(expected, actual));
}
///* ************************************************************************* */
//SL-FIX TEST( GaussianFactorGraph, constrained_multi2 )
//{
// // get a graph with a constraint in it
// GaussianFactorGraph fg = createMultiConstraintGraph();
//
// // eliminate and solve
// Ordering ord;
// ord += "zk, xk, y";
// VectorValues actual = fg.optimize(ord);
//
// // verify
// VectorValues expected = createMultiConstraintValues();
// EXPECT(assert_equal(expected, actual));
//}
/* ************************************************************************* */
SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
//{
// GaussianFactorGraph g;
// Matrix I = eye(2);
// Vector b = Vector_(0, 0, 0);
// g.add(X(1), I, X(2), I, b, model);
// g.add(X(1), I, X(3), I, b, model);
// g.add(X(1), I, X(4), I, b, model);
// g.add(X(2), I, X(3), I, b, model);
// g.add(X(2), I, X(4), I, b, model);
// g.add(X(3), I, X(4), I, b, model);
//
// map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
// EXPECT(tree[X(1)].compare(X(1))==0);
// EXPECT(tree[X(2)].compare(X(1))==0);
// EXPECT(tree[X(3)].compare(X(1))==0);
// EXPECT(tree[X(4)].compare(X(1))==0);
//}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, split )
//{
// GaussianFactorGraph g;
// Matrix I = eye(2);
// Vector b = Vector_(0, 0, 0);
// g.add(X(1), I, X(2), I, b, model);
// g.add(X(1), I, X(3), I, b, model);
// g.add(X(1), I, X(4), I, b, model);
// g.add(X(2), I, X(3), I, b, model);
// g.add(X(2), I, X(4), I, b, model);
//
// PredecessorMap<string> tree;
// tree[X(1)] = X(1);
// tree[X(2)] = X(1);
// tree[X(3)] = X(1);
// tree[X(4)] = X(1);
//
// GaussianFactorGraph Ab1, Ab2;
// g.split<string, GaussianFactor>(tree, Ab1, Ab2);
// LONGS_EQUAL(3, Ab1.size());
// LONGS_EQUAL(2, Ab2.size());
//}
static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
/* ************************************************************************* */
TEST(GaussianFactorGraph, replace)
@ -935,91 +482,18 @@ TEST(GaussianFactorGraph, replace)
GaussianFactorGraph actual;
actual.push_back(f1);
// actual.checkGraphConsistency();
actual.push_back(f2);
// actual.checkGraphConsistency();
actual.push_back(f3);
// actual.checkGraphConsistency();
actual.replace(0, f4);
// actual.checkGraphConsistency();
GaussianFactorGraph expected;
expected.push_back(f4);
// actual.checkGraphConsistency();
expected.push_back(f2);
// actual.checkGraphConsistency();
expected.push_back(f3);
// actual.checkGraphConsistency();
EXPECT(assert_equal(expected, actual));
}
///* ************************************************************************* */
//TEST ( GaussianFactorGraph, combine_matrix ) {
// // create a small linear factor graph
// GaussianFactorGraph fg = createGaussianFactorGraph();
// Dimensions dimensions = fg.dimensions();
//
// // get two factors from it and insert the factors into a vector
// vector<GaussianFactor::shared_ptr> lfg;
// lfg.push_back(fg[4 - 1]);
// lfg.push_back(fg[2 - 1]);
//
// // combine in a factor
// Matrix Ab; SharedDiagonal noise;
// Ordering order; order += X(2), L(1), X(1);
// boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions);
//
// // the expected augmented matrix
// Matrix expAb = Matrix_(4, 7,
// -5., 0., 5., 0., 0., 0.,-1.0,
// +0., -5., 0., 5., 0., 0., 1.5,
// 10., 0., 0., 0.,-10., 0., 2.0,
// +0., 10., 0., 0., 0.,-10.,-1.0);
//
// // expected noise model
// SharedDiagonal expModel = noiseModel::Unit::Create(4);
//
// EXPECT(assert_equal(expAb, Ab));
// EXPECT(assert_equal(*expModel, *noise));
//}
/* ************************************************************************* */
/*
* x2 x1 x3 b
* 1 1 1 1 1 0 1
* 1 1 1 -> 1 1 1
* 1 1 1 1
*/
// SL-NEEDED? TEST ( GaussianFactorGraph, eliminateFrontals ) {
// typedef GaussianFactorGraph::sharedFactor Factor;
// SharedDiagonal model(Vector_(1, 0.5));
// GaussianFactorGraph fg;
// Factor factor1(new JacobianFactor(X(1), Matrix_(1,1,1.), X(2), Matrix_(1,1,1.), Vector_(1,1.), model));
// Factor factor2(new JacobianFactor(X(2), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model));
// Factor factor3(new JacobianFactor(X(3), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model));
// fg.push_back(factor1);
// fg.push_back(factor2);
// fg.push_back(factor3);
//
// Ordering frontals; frontals += X(2), X(1);
// GaussianBayesNet bn = fg.eliminateFrontals(frontals);
//
// GaussianBayesNet bn_expected;
// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(X(2), Vector_(1, 2.), Matrix_(1, 1, 2.),
// X(1), Matrix_(1, 1, 1.), X(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(X(1), Vector_(1, 0.), Matrix_(1, 1, -1.),
// X(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
// bn_expected.push_back(conditional1);
// bn_expected.push_back(conditional2);
// EXPECT(assert_equal(bn_expected, bn));
//
// GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(X(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.))));
// GaussianFactorGraph fg_expected;
// fg_expected.push_back(factor_expected);
// EXPECT(assert_equal(fg_expected, fg));
//}
/* ************************************************************************* */
TEST(GaussianFactorGraph, createSmoother2)
{
@ -1049,7 +523,7 @@ TEST(GaussianFactorGraph, hasConstraints)
EXPECT(hasConstraints(fgc2));
Ordering ordering; ordering += X(1), X(2), L(1);
FactorGraph<GaussianFactor> fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
EXPECT(!hasConstraints(fg));
}

29
tests/testGaussianISAM.cpp
View File

@ -42,12 +42,12 @@ using symbol_shorthand::L;
// Some numbers that should be consistent among all smoother tests
static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1;
static const double tol = 1e-4;
/* ************************************************************************* */
TEST_UNSAFE( ISAM, iSAM_smoother )
TEST( ISAM, iSAM_smoother )
{
Ordering ordering;
for (int t = 1; t <= 7; t++) ordering += X(t);
@ -76,31 +76,6 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
EXPECT(assert_equal(e, optimized));
}
/* ************************************************************************* */
// SL-FIX TEST( ISAM, iSAM_smoother2 )
//{
// // Create smoother with 7 nodes
// GaussianFactorGraph smoother = createSmoother(7);
//
// // Create initial tree from first 4 timestamps in reverse order !
// Ordering ord; ord += X(4),X(3),X(2),X(1);
// GaussianFactorGraph factors1;
// for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
// GaussianISAM actual(*inference::Eliminate(factors1));
//
// // run iSAM with remaining factors
// GaussianFactorGraph factors2;
// for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
// actual.update(factors2);
//
// // Create expected Bayes Tree by solving smoother with "natural" ordering
// Ordering ordering;
// for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
// GaussianISAM expected(smoother.eliminate(ordering));
//
// EXPECT(assert_equal(expected, actual));
//}
/* ************************************************************************* *
Bayes tree for smoother with "natural" ordering:
C1 x6 x7

2
tests/testGaussianISAM2.cpp
View File

@ -12,7 +12,7 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

102
tests/testGraph.cpp
View File

@ -105,6 +105,108 @@ TEST( Graph, composePoses )
CHECK(assert_equal(expected, *actual));
}
// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
//{
// GaussianFactorGraph g;
// Matrix I = eye(2);
// Vector b = Vector_(0, 0, 0);
// g.add(X(1), I, X(2), I, b, model);
// g.add(X(1), I, X(3), I, b, model);
// g.add(X(1), I, X(4), I, b, model);
// g.add(X(2), I, X(3), I, b, model);
// g.add(X(2), I, X(4), I, b, model);
// g.add(X(3), I, X(4), I, b, model);
//
// map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
// EXPECT(tree[X(1)].compare(X(1))==0);
// EXPECT(tree[X(2)].compare(X(1))==0);
// EXPECT(tree[X(3)].compare(X(1))==0);
// EXPECT(tree[X(4)].compare(X(1))==0);
//}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, split )
//{
// GaussianFactorGraph g;
// Matrix I = eye(2);
// Vector b = Vector_(0, 0, 0);
// g.add(X(1), I, X(2), I, b, model);
// g.add(X(1), I, X(3), I, b, model);
// g.add(X(1), I, X(4), I, b, model);
// g.add(X(2), I, X(3), I, b, model);
// g.add(X(2), I, X(4), I, b, model);
//
// PredecessorMap<string> tree;
// tree[X(1)] = X(1);
// tree[X(2)] = X(1);
// tree[X(3)] = X(1);
// tree[X(4)] = X(1);
//
// GaussianFactorGraph Ab1, Ab2;
// g.split<string, GaussianFactor>(tree, Ab1, Ab2);
// LONGS_EQUAL(3, Ab1.size());
// LONGS_EQUAL(2, Ab2.size());
//}
///* ************************************************************************* */
// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree )
//{
// SymbolicFactorGraph G;
// G.push_factor("x1", "x2");
// G.push_factor("x1", "x3");
// G.push_factor("x1", "x4");
// G.push_factor("x2", "x3");
// G.push_factor("x2", "x4");
// G.push_factor("x3", "x4");
//
// SymbolicFactorGraph T, C;
// boost::tie(T, C) = G.splitMinimumSpanningTree();
//
// SymbolicFactorGraph expectedT, expectedC;
// expectedT.push_factor("x1", "x2");
// expectedT.push_factor("x1", "x3");
// expectedT.push_factor("x1", "x4");
// expectedC.push_factor("x2", "x3");
// expectedC.push_factor("x2", "x4");
// expectedC.push_factor("x3", "x4");
// CHECK(assert_equal(expectedT,T));
// CHECK(assert_equal(expectedC,C));
//}
///* ************************************************************************* */
///**
// * x1 - x2 - x3 - x4 - x5
// * | | / |
// * l1 l2 l3
// */
// SL-FIX TEST( FactorGraph, removeSingletons )
//{
// SymbolicFactorGraph G;
// G.push_factor("x1", "x2");
// G.push_factor("x2", "x3");
// G.push_factor("x3", "x4");
// G.push_factor("x4", "x5");
// G.push_factor("x2", "l1");
// G.push_factor("x3", "l2");
// G.push_factor("x4", "l2");
// G.push_factor("x4", "l3");
//
// SymbolicFactorGraph singletonGraph;
// set<Symbol> singletons;
// boost::tie(singletonGraph, singletons) = G.removeSingletons();
//
// set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
// CHECK(singletons_excepted == singletons);
//
// SymbolicFactorGraph singletonGraph_excepted;
// singletonGraph_excepted.push_factor("x2", "l1");
// singletonGraph_excepted.push_factor("x4", "l3");
// singletonGraph_excepted.push_factor("x1", "x2");
// singletonGraph_excepted.push_factor("x4", "x5");
// singletonGraph_excepted.push_factor("x2", "x3");
// CHECK(singletonGraph_excepted.equals(singletonGraph));
//}
/* ************************************************************************* */
int main() {
TestResult tr;

154
tests/testIterative.cpp Normal file
View File

@ -0,0 +1,154 @@
/* ----------------------------------------------------------------------------
* 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 testIterative.cpp
* @brief Unit tests for iterative methods
* @author Frank Dellaert
**/
#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/geometry/Pose2.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace example;
using symbol_shorthand::X; // to create pose keys
using symbol_shorthand::L; // to create landmark keys
static ConjugateGradientParameters parameters;
// add following below to add printing:
// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY;
/* ************************************************************************* */
TEST( Iterative, steepestDescent )
{
// Create factor graph
Ordering ordering;
ordering += L(1), X(1), X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// eliminate and solve
VectorValues expected = *GaussianSequentialSolver(fg).optimize();
// Do gradient descent
VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally?
VectorValues actual = steepestDescent(fg, zero, parameters);
CHECK(assert_equal(expected,actual,1e-2));
}
/* ************************************************************************* */
TEST( Iterative, conjugateGradientDescent )
{
// Create factor graph
Ordering ordering;
ordering += L(1), X(1), X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// eliminate and solve
VectorValues expected = *GaussianSequentialSolver(fg).optimize();
// get matrices
Matrix A;
Vector b;
Vector x0 = gtsam::zero(6);
boost::tie(A, b) = fg.jacobian();
Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2);
// Do conjugate gradient descent, System version
System Ab(A, b);
Vector actualX = conjugateGradientDescent(Ab, x0, parameters);
CHECK(assert_equal(expectedX,actualX,1e-9));
// Do conjugate gradient descent, Matrix version
Vector actualX2 = conjugateGradientDescent(A, b, x0, parameters);
CHECK(assert_equal(expectedX,actualX2,1e-9));
// Do conjugate gradient descent on factor graph
VectorValues zero = VectorValues::Zero(expected);
VectorValues actual = conjugateGradientDescent(fg, zero, parameters);
CHECK(assert_equal(expected,actual,1e-2));
}
/* ************************************************************************* */
TEST( Iterative, conjugateGradientDescent_hard_constraint )
{
Values config;
Pose2 pose1 = Pose2(0.,0.,0.);
config.insert(X(1), pose1);
config.insert(X(2), Pose2(1.5,0.,0.));
NonlinearFactorGraph graph;
graph.add(NonlinearEquality<Pose2>(X(1), pose1));
graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
Ordering ordering;
ordering += X(1), X(2);
boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config,ordering);
VectorValues zeros = VectorValues::Zero(2, 3);
ConjugateGradientParameters parameters;
parameters.setEpsilon_abs(1e-3);
parameters.setEpsilon_rel(1e-5);
parameters.setMaxIterations(100);
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValues expected;
expected.insert(0, zero(3));
expected.insert(1, Vector_(3,-0.5,0.,0.));
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( Iterative, conjugateGradientDescent_soft_constraint )
{
Values config;
config.insert(X(1), Pose2(0.,0.,0.));
config.insert(X(2), Pose2(1.5,0.,0.));
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
Ordering ordering;
ordering += X(1), X(2);
boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config,ordering);
VectorValues zeros = VectorValues::Zero(2, 3);
ConjugateGradientParameters parameters;
parameters.setEpsilon_abs(1e-3);
parameters.setEpsilon_rel(1e-5);
parameters.setMaxIterations(100);
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValues expected;
expected.insert(0, zero(3));
expected.insert(1, Vector_(3,-0.5,0.,0.));
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
tests/testNonlinearFactor.cpp
View File

@ -73,8 +73,8 @@ TEST( NonlinearFactor, equals2 )
Graph::sharedFactor f0 = fg[0], f1 = fg[1];
CHECK(f0->equals(*f0));
// SL-FIX CHECK(!f0->equals(*f1));
// SL-FIX CHECK(!f1->equals(*f0));
CHECK(!f0->equals(*f1));
CHECK(!f1->equals(*f0));
}
/* ************************************************************************* */

304
tests/testSubgraphPreconditioner.cpp
View File

@ -17,8 +17,9 @@
#include <tests/smallExample.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/base/numericalDerivative.h>
@ -35,19 +36,30 @@ using namespace gtsam;
using namespace example;
// define keys
Key i3003 = 3003, i2003 = 2003, i1003 = 1003;
Key i3002 = 3002, i2002 = 2002, i1002 = 1002;
Key i3001 = 3001, i2001 = 2001, i1001 = 1001;
// Create key for simulated planar graph
Symbol key(int x, int y) {
return symbol_shorthand::X(1000*x+y);
}
// TODO fix Ordering::equals, because the ordering *is* correct !
/* ************************************************************************* */
//TEST( SubgraphPreconditioner, planarOrdering )
//{
// // Check canonical ordering
// Ordering expected, ordering = planarOrdering(3);
// expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001;
// CHECK(assert_equal(expected,ordering));
//}
TEST( SubgraphPreconditioner, planarOrdering ) {
// Check canonical ordering
Ordering expected, ordering = planarOrdering(3);
expected +=
key(3, 3), key(2, 3), key(1, 3),
key(3, 2), key(2, 2), key(1, 2),
key(3, 1), key(2, 1), key(1, 1);
CHECK(assert_equal(expected,ordering));
}
/* ************************************************************************* */
/** unnormalized error */
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
total_error += factor->error(x);
return total_error;
}
/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarGraph )
@ -58,7 +70,7 @@ TEST( SubgraphPreconditioner, planarGraph )
boost::tie(A, xtrue) = planarGraph(3);
LONGS_EQUAL(13,A.size());
LONGS_EQUAL(9,xtrue.size());
DOUBLES_EQUAL(0,A.error(xtrue),1e-9); // check zero error for xtrue
DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue
// Check that xtrue is optimal
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
@ -67,143 +79,143 @@ TEST( SubgraphPreconditioner, planarGraph )
}
/* ************************************************************************* */
//TEST( SubgraphPreconditioner, splitOffPlanarTree )
//{
// // Build a planar graph
// GaussianFactorGraph A;
// VectorValues xtrue;
// boost::tie(A, xtrue) = planarGraph(3);
//
// // Get the spanning tree and constraints, and check their sizes
// JacobianFactorGraph T, C;
// // TODO big mess: GFG and JFG mess !!!
// boost::tie(T, C) = splitOffPlanarTree(3, A);
// LONGS_EQUAL(9,T.size());
// LONGS_EQUAL(4,C.size());
//
// // Check that the tree can be solved to give the ground xtrue
// GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
// VectorValues xbar = optimize(*R1);
// CHECK(assert_equal(xtrue,xbar));
//}
TEST( SubgraphPreconditioner, splitOffPlanarTree )
{
// Build a planar graph
GaussianFactorGraph A;
VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3);
// Get the spanning tree and constraints, and check their sizes
GaussianFactorGraph T, C;
boost::tie(T, C) = splitOffPlanarTree(3, A);
LONGS_EQUAL(9,T.size());
LONGS_EQUAL(4,C.size());
// Check that the tree can be solved to give the ground xtrue
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
VectorValues xbar = optimize(*R1);
CHECK(assert_equal(xtrue,xbar));
}
/* ************************************************************************* */
//TEST( SubgraphPreconditioner, system )
//{
// // Build a planar graph
// JacobianFactorGraph Ab;
// VectorValues xtrue;
// size_t N = 3;
// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
//
// // Get the spanning tree and corresponding ordering
// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
//
// // Eliminate the spanning tree to build a prior
// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
//
// // Create Subgraph-preconditioned system
// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
//
// // Create zero config
// VectorValues zeros = VectorValues::Zero(xbar);
//
// // Set up y0 as all zeros
// VectorValues y0 = zeros;
//
// // y1 = perturbed y0
// VectorValues y1 = zeros;
// y1[i2003] = Vector_(2, 1.0, -1.0);
//
// // Check corresponding x values
// VectorValues expected_x1 = xtrue, x1 = system.x(y1);
// expected_x1[i2003] = Vector_(2, 2.01, 2.99);
// expected_x1[i3003] = Vector_(2, 3.01, 2.99);
// CHECK(assert_equal(xtrue, system.x(y0)));
// CHECK(assert_equal(expected_x1,system.x(y1)));
//
// // Check errors
//// DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); // TODO !
//// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO !
// DOUBLES_EQUAL(0,error(system,y0),1e-9);
// DOUBLES_EQUAL(3,error(system,y1),1e-9);
//
// // Test gradient in x
// VectorValues expected_gx0 = zeros;
// VectorValues expected_gx1 = zeros;
// CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
// expected_gx1[i1003] = Vector_(2, -100., 100.);
// expected_gx1[i2002] = Vector_(2, -100., 100.);
// expected_gx1[i2003] = Vector_(2, 200., -200.);
// expected_gx1[i3002] = Vector_(2, -100., 100.);
// expected_gx1[i3003] = Vector_(2, 100., -100.);
// CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
//
// // Test gradient in y
// VectorValues expected_gy0 = zeros;
// VectorValues expected_gy1 = zeros;
// expected_gy1[i1003] = Vector_(2, 2., -2.);
// expected_gy1[i2002] = Vector_(2, -2., 2.);
// expected_gy1[i2003] = Vector_(2, 3., -3.);
// expected_gy1[i3002] = Vector_(2, -1., 1.);
// expected_gy1[i3003] = Vector_(2, 1., -1.);
// CHECK(assert_equal(expected_gy0,gradient(system,y0)));
// CHECK(assert_equal(expected_gy1,gradient(system,y1)));
//
// // Check it numerically for good measure
// // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
// // Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001);
// // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
// // 3., -3., 0., 0., -1., 1., 1., -1.);
// // CHECK(assert_equal(expected_g1,numerical_g1));
//}
TEST( SubgraphPreconditioner, system )
{
// Build a planar graph
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// Eliminate the spanning tree to build a prior
SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
// Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config
VectorValues zeros = VectorValues::Zero(xbar);
// Set up y0 as all zeros
VectorValues y0 = zeros;
// y1 = perturbed y0
VectorValues y1 = zeros;
y1[1] = Vector_(2, 1.0, -1.0);
// Check corresponding x values
VectorValues expected_x1 = xtrue, x1 = system.x(y1);
expected_x1[1] = Vector_(2, 2.01, 2.99);
expected_x1[0] = Vector_(2, 3.01, 2.99);
CHECK(assert_equal(xtrue, system.x(y0)));
CHECK(assert_equal(expected_x1,system.x(y1)));
// Check errors
DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9);
DOUBLES_EQUAL(3,error(Ab,x1),1e-9);
DOUBLES_EQUAL(0,error(system,y0),1e-9);
DOUBLES_EQUAL(3,error(system,y1),1e-9);
// Test gradient in x
VectorValues expected_gx0 = zeros;
VectorValues expected_gx1 = zeros;
CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
expected_gx1[2] = Vector_(2, -100., 100.);
expected_gx1[4] = Vector_(2, -100., 100.);
expected_gx1[1] = Vector_(2, 200., -200.);
expected_gx1[3] = Vector_(2, -100., 100.);
expected_gx1[0] = Vector_(2, 100., -100.);
CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
// Test gradient in y
VectorValues expected_gy0 = zeros;
VectorValues expected_gy1 = zeros;
expected_gy1[2] = Vector_(2, 2., -2.);
expected_gy1[4] = Vector_(2, -2., 2.);
expected_gy1[1] = Vector_(2, 3., -3.);
expected_gy1[3] = Vector_(2, -1., 1.);
expected_gy1[0] = Vector_(2, 1., -1.);
CHECK(assert_equal(expected_gy0,gradient(system,y0)));
CHECK(assert_equal(expected_gy1,gradient(system,y1)));
// Check it numerically for good measure
// TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
// Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001);
// Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
// 3., -3., 0., 0., -1., 1., 1., -1.);
// CHECK(assert_equal(expected_g1,numerical_g1));
}
/* ************************************************************************* */
//TEST( SubgraphPreconditioner, conjugateGradients )
//{
// // Build a planar graph
// GaussianFactorGraph Ab;
// VectorValues xtrue;
// size_t N = 3;
// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
//
// // Get the spanning tree and corresponding ordering
// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
//
// // Eliminate the spanning tree to build a prior
// Ordering ordering = planarOrdering(N);
// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
//
// // Create Subgraph-preconditioned system
// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
//
// // Create zero config y0 and perturbed config y1
// VectorValues y0 = VectorValues::Zero(xbar);
//
// VectorValues y1 = y0;
// y1[i2003] = Vector_(2, 1.0, -1.0);
// VectorValues x1 = system.x(y1);
//
// // Solve for the remaining constraints using PCG
// ConjugateGradientParameters parameters;
//// VectorValues actual = gtsam::conjugateGradients<SubgraphPreconditioner,
//// VectorValues, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations);
//// CHECK(assert_equal(y0,actual));
//
// // Compare with non preconditioned version:
// VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
// CHECK(assert_equal(xtrue,actual2,1e-4));
//}
TEST( SubgraphPreconditioner, conjugateGradients )
{
// Build a planar graph
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// Eliminate the spanning tree to build a prior
Ordering ordering = planarOrdering(N);
SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
// Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config y0 and perturbed config y1
VectorValues y0 = VectorValues::Zero(xbar);
VectorValues y1 = y0;
y1[1] = Vector_(2, 1.0, -1.0);
VectorValues x1 = system.x(y1);
// Solve for the remaining constraints using PCG
ConjugateGradientParameters parameters;
VectorValues actual = conjugateGradients<SubgraphPreconditioner,
VectorValues, Errors>(system, y1, parameters);
CHECK(assert_equal(y0,actual));
// Compare with non preconditioned version:
VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
CHECK(assert_equal(xtrue,actual2,1e-4));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

114
tests/testSubgraphSolver.cpp Normal file
View File

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testSubgraphSolver.cpp
* @brief Unit tests for SubgraphSolver
* @author Yong-Dian Jian
**/
#include <tests/smallExample.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
/** unnormalized error */
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
total_error += factor->error(x);
return total_error;
}
/* ************************************************************************* */
TEST( SubgraphSolver, constructor1 )
{
// Build a planar graph
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// The first constructor just takes a factor graph (and parameters)
// and it will split the graph into A1 and A2, where A1 is a spanning tree
SubgraphSolverParameters parameters;
SubgraphSolver solver(Ab, parameters);
VectorValues optimized = solver.optimize(); // does PCG optimization
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
/* ************************************************************************* */
TEST( SubgraphSolver, constructor2 )
{
// Build a planar graph
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// The second constructor takes two factor graphs,
// so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2)
SubgraphSolverParameters parameters;
SubgraphSolver solver(Ab1_, Ab2_, parameters);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
/* ************************************************************************* */
TEST( SubgraphSolver, constructor3 )
{
// Build a planar graph
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT
GaussianBayesNet::shared_ptr Rc1 = //
EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR);
// The third constructor allows the caller to pass an already solved preconditioner Rc1_
// as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
SubgraphSolverParameters parameters;
SubgraphSolver solver(Rc1, Ab2_, parameters);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

6
wrap/Class.cpp
View File

@ -332,7 +332,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const
{
string up_name = boost::to_upper_copy(name);
proxyFile.oss << "%" << up_name << "(";
proxyFile.oss << "%" << name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList)
{
@ -352,7 +352,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const
BOOST_FOREACH(ArgumentList argList, m.argLists)
{
string up_name = boost::to_upper_copy(m.name);
proxyFile.oss << "%" << up_name << "(";
proxyFile.oss << "%" << m.name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList)
{
@ -373,7 +373,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const
BOOST_FOREACH(ArgumentList argList, m.argLists)
{
string up_name = boost::to_upper_copy(m.name);
proxyFile.oss << "%" << up_name << "(";
proxyFile.oss << "%" << m.name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList)
{

18
wrap/tests/expected/Point2.m
View File

@ -1,15 +1,15 @@
%-------Constructors-------
%POINT2()
%POINT2(double x, double y)
%Point2()
%Point2(double x, double y)
%
%-------Methods-------
%ARGCHAR(char a) : returns void
%ARGUCHAR(unsigned char a) : returns void
%DIM() : returns int
%RETURNCHAR() : returns char
%VECTORCONFUSION() : returns VectorNotEigen
%X() : returns double
%Y() : returns double
%argChar(char a) : returns void
%argUChar(unsigned char a) : returns void
%dim() : returns int
%returnChar() : returns char
%vectorConfusion() : returns VectorNotEigen
%x() : returns double
%y() : returns double
%
%-------Static Methods-------
%

8
wrap/tests/expected/Point3.m
View File

@ -1,12 +1,12 @@
%-------Constructors-------
%POINT3(double x, double y, double z)
%Point3(double x, double y, double z)
%
%-------Methods-------
%NORM() : returns double
%norm() : returns double
%
%-------Static Methods-------
%STATICFUNCTIONRET(double z) : returns Point3
%STATICFUNCTION() : returns double
%StaticFunctionRet(double z) : returns Point3
%staticFunction() : returns double
%
%For more detailed documentation on GTSAM go to our Doxygen page, which can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
classdef Point3 < handle

42
wrap/tests/expected/Test.m
View File

@ -1,27 +1,27 @@
%-------Constructors-------
%TEST()
%TEST(double a, Matrix b)
%Test()
%Test(double a, Matrix b)
%
%-------Methods-------
%ARG_EIGENCONSTREF(Matrix value) : returns void
%CREATE_MIXEDPTRS() : returns pair< Test, SharedTest >
%CREATE_PTRS() : returns pair< SharedTest, SharedTest >
%PRINT() : returns void
%RETURN_POINT2PTR(bool value) : returns Point2
%RETURN_TEST(Test value) : returns Test
%RETURN_TESTPTR(Test value) : returns Test
%RETURN_BOOL(bool value) : returns bool
%RETURN_DOUBLE(double value) : returns double
%RETURN_FIELD(Test t) : returns bool
%RETURN_INT(int value) : returns int
%RETURN_MATRIX1(Matrix value) : returns Matrix
%RETURN_MATRIX2(Matrix value) : returns Matrix
%RETURN_PAIR(Vector v, Matrix A) : returns pair< Vector, Matrix >
%RETURN_PTRS(Test p1, Test p2) : returns pair< SharedTest, SharedTest >
%RETURN_SIZE_T(size_t value) : returns size_t
%RETURN_STRING(string value) : returns string
%RETURN_VECTOR1(Vector value) : returns Vector
%RETURN_VECTOR2(Vector value) : returns Vector
%arg_EigenConstRef(Matrix value) : returns void
%create_MixedPtrs() : returns pair< Test, SharedTest >
%create_ptrs() : returns pair< SharedTest, SharedTest >
%print() : returns void
%return_Point2Ptr(bool value) : returns Point2
%return_Test(Test value) : returns Test
%return_TestPtr(Test value) : returns Test
%return_bool(bool value) : returns bool
%return_double(double value) : returns double
%return_field(Test t) : returns bool
%return_int(int value) : returns int
%return_matrix1(Matrix value) : returns Matrix
%return_matrix2(Matrix value) : returns Matrix
%return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
%return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest >
%return_size_t(size_t value) : returns size_t
%return_string(string value) : returns string
%return_vector1(Vector value) : returns Vector
%return_vector2(Vector value) : returns Vector
%
%-------Static Methods-------
%

2
wrap/tests/expected_namespaces/+ns1/ClassA.m
View File

@ -1,5 +1,5 @@
%-------Constructors-------
%CLASSA()
%ClassA()
%
%-------Methods-------
%

2
wrap/tests/expected_namespaces/+ns1/ClassB.m
View File

@ -1,5 +1,5 @@
%-------Constructors-------
%CLASSB()
%ClassB()
%
%-------Methods-------
%

2
wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m
View File

@ -1,5 +1,5 @@
%-------Constructors-------
%CLASSB()
%ClassB()
%
%-------Methods-------
%

10
wrap/tests/expected_namespaces/+ns2/ClassA.m
View File

@ -1,13 +1,13 @@
%-------Constructors-------
%CLASSA()
%ClassA()
%
%-------Methods-------
%MEMBERFUNCTION() : returns double
%NSARG(ClassB arg) : returns int
%NSRETURN(double q) : returns ns2::ns3::ClassB
%memberFunction() : returns double
%nsArg(ClassB arg) : returns int
%nsReturn(double q) : returns ns2::ns3::ClassB
%
%-------Static Methods-------
%AFUNCTION() : returns double
%afunction() : returns double
%
%For more detailed documentation on GTSAM go to our Doxygen page, which can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
classdef ClassA < handle

2
wrap/tests/expected_namespaces/+ns2/ClassC.m
View File

@ -1,5 +1,5 @@
%-------Constructors-------
%CLASSC()
%ClassC()
%
%-------Methods-------
%

2
wrap/tests/expected_namespaces/ClassD.m
View File

@ -1,5 +1,5 @@
%-------Constructors-------
%CLASSD()
%ClassD()
%
%-------Methods-------
%