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

Removed functions largely supersceded by newer bayes tree functionality

release/4.3a0
Alex Cunningham 2013-08-16 16:32:19 +00:00
parent 0cdb233e5f
commit 2d413e05c6
10 changed files with 197 additions and 1368 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

374
.cproject
View File

@ -315,14 +315,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>
@ -349,6 +341,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>
@ -356,6 +349,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>
@ -403,6 +397,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>
@ -410,6 +405,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>
@ -417,6 +413,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>
@ -432,11 +429,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>
@ -525,22 +531,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>
@ -557,6 +547,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>
@ -581,42 +587,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>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testLinearContainerFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testWhiteNoiseFactor.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -661,26 +651,42 @@
<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>
</target>
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testLinearContainerFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testWhiteNoiseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1071,6 +1077,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>
@ -1078,6 +1085,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>
@ -1085,6 +1093,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>
@ -1252,6 +1261,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>
@ -1297,14 +1307,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/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="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1385,6 +1387,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/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="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1691,7 +1701,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>
@ -1731,7 +1740,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>
@ -1739,7 +1747,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>
@ -1849,6 +1856,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianBayesNetUnordered.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1963,7 +1978,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>
@ -1985,102 +1999,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>-j3</buildArguments>
@ -2282,6 +2200,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>
@ -2289,6 +2208,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>
@ -2296,6 +2216,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>
@ -2303,6 +2224,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>
@ -2468,34 +2390,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>
@ -2539,6 +2525,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>

4
gtsam_unstable/CMakeLists.txt
View File

@ -4,7 +4,6 @@ set (gtsam_unstable_subdirs
base
geometry
#discrete
linear
dynamics
nonlinear
slam
@ -53,8 +52,7 @@ set(gtsam_unstable_srcs
${base_srcs}
${geometry_srcs}
#${discrete_srcs}
${dynamics_srcs}
${linear_srcs}
${dynamics_srcs}
${nonlinear_srcs}
${slam_srcs}
)

25
gtsam_unstable/linear/CMakeLists.txt
View File

@ -1,25 +0,0 @@
# Install headers
file(GLOB linear_headers "*.h")
install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear)
# Components to link tests in this subfolder against
set(linear_local_libs
linear_unstable
nonlinear
linear
inference
geometry
base
ccolamd
)
set (linear_full_libs
${gtsam-default}
${gtsam_unstable-default})
# Exclude tests that don't work
set (base_excluded_tests "")
# Add all tests
gtsam_add_subdir_tests(linear_unstable "${linear_local_libs}" "${linear_full_libs}" "${linear_excluded_tests}")
add_dependencies(check.unstable check.linear_unstable)

126
gtsam_unstable/linear/bayesTreeOperations.cpp
View File

@ -1,126 +0,0 @@
/**
* @file bayesTreeOperations.cpp
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#include <gtsam_unstable/linear/bayesTreeOperations.h>
#include <boost/foreach.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph) {
GaussianFactorGraph result;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) {
GaussianFactorGraph split = splitFactor(factor);
result.push_back(split);
}
return result;
}
/* ************************************************************************* */
GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) {
GaussianFactorGraph result;
if (!factor) return result;
// Needs to be a jacobian factor - just pass along hessians
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
if (!jf) {
result.push_back(factor);
return result;
}
// Loop over variables and strip off factors using split conditionals
// Assumes upper triangular structure
JacobianFactor::const_iterator rowIt, colIt;
const size_t totalRows = jf->rows();
size_t rowsRemaining = totalRows;
for (rowIt = jf->begin(); rowIt != jf->end() && rowsRemaining > 0; ++rowIt) {
// get dim of current variable
size_t varDim = jf->getDim(rowIt);
size_t startRow = totalRows - rowsRemaining;
size_t nrRows = std::min(rowsRemaining, varDim);
// Extract submatrices
std::vector<std::pair<Key, Matrix> > matrices;
for (colIt = rowIt; colIt != jf->end(); ++colIt) {
Key idx = *colIt;
const Matrix subA = jf->getA(colIt).middleRows(startRow, nrRows);
matrices.push_back(make_pair(idx, subA));
}
Vector subB = jf->getb().segment(startRow, nrRows);
SharedDiagonal model;
if(jf->get_model()) {
Vector sigmas = jf->get_model()->sigmas().segment(startRow, nrRows);
if (jf->get_model()->isConstrained())
model = noiseModel::Constrained::MixedSigmas(sigmas);
else
model = noiseModel::Diagonal::Sigmas(sigmas);
}
// extract matrices from each
// assemble into new JacobianFactor
result.add(matrices, subB, model);
rowsRemaining -= nrRows;
}
return result;
}
/* ************************************************************************* */
GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph) {
GaussianFactorGraph result;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
if (factor && (!jf || jf->size() > 1))
result.push_back(factor);
}
return result;
}
/* ************************************************************************* */
void findCliques(const GaussianBayesTree::sharedClique& current_clique,
std::set<GaussianConditional::shared_ptr>& result) {
// Add the current clique
result.insert(current_clique->conditional());
// Add the parent if not root
if (!current_clique->isRoot())
findCliques(current_clique->parent(), result);
}
/* ************************************************************************* */
std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTree& bayesTree, const std::set<Key>& savedIndices) {
std::set<GaussianConditional::shared_ptr> affected_cliques;
// FIXME: track previously found keys more efficiently
BOOST_FOREACH(const Key& index, savedIndices) {
GaussianBayesTree::sharedClique clique = bayesTree[index];
// add path back to root to affected set
findCliques(clique, affected_cliques);
}
return affected_cliques;
}
/* ************************************************************************* */
std::deque<GaussianBayesTree::sharedClique>
findPathCliques(const GaussianBayesTree::sharedClique& initial) {
std::deque<GaussianBayesTree::sharedClique> result, parents;
if (initial->isRoot())
return result;
result.push_back(initial->parent());
parents = findPathCliques(initial->parent());
result.insert(result.end(), parents.begin(), parents.end());
return result;
}
/* ************************************************************************* */
} // \namespace gtsam

99
gtsam_unstable/linear/bayesTreeOperations.h
View File

@ -1,99 +0,0 @@
/**
* @file bayesTreeOperations.h
*
* @brief Types and utility functions for operating on linear systems
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#pragma once
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>
namespace gtsam {
// Linear Graph Operations
/**
* Given a graph, splits each factor into factors where the dimension is
* that of the first variable.
*/
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph);
/**
* Splits a factor into factors where the dimension is
* that of the first variable.
*/
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor);
/** Removes prior jacobian factors from the graph */
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph);
// Bayes Tree / Conditional operations
/**
* Given a Bayes Tree, return conditionals corresponding to cliques that have or
* are connected to a set of wanted variables.
*
* @return the set of conditionals extracted from cliques.
*/
GTSAM_UNSTABLE_EXPORT std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTree& bayesTree, const std::set<Key>& savedIndices);
/**
* Recursively traverses from a given clique in a Bayes Tree and collects all of the conditionals
* Adds any new cliques from path to root to the result set.
*
* Note the use of a set of shared_ptr: this will sort/filter on unique *pointer* locations,
* which ensures unique cliques, but the order of the cliques is meaningless
*/
GTSAM_UNSTABLE_EXPORT void findCliqueConditionals(const GaussianBayesTree::sharedClique& current_clique,
std::set<GaussianConditional::shared_ptr>& result);
/**
* Given a clique, returns a sequence of clique parents to the root, not including the
* given clique.
*/
GTSAM_UNSTABLE_EXPORT std::deque<GaussianBayesTree::sharedClique>
findPathCliques(const GaussianBayesTree::sharedClique& initial);
/**
* Liquefies a BayesTree into a GaussianFactorGraph recursively, given a
* root clique
*
* @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors
*/
template <class BAYESTREE>
GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) {
GaussianFactorGraph result;
if (root && root->conditional()) {
GaussianConditional::shared_ptr conditional = root->conditional();
if (!splitConditionals)
result.push_back(conditional);
else
result.push_back(splitFactor(conditional));
}
BOOST_FOREACH(typename BAYESTREE::sharedClique child, root->children)
result.push_back(liquefy<BAYESTREE>(child, splitConditionals));
return result;
}
/**
* Liquefies a BayesTree into a GaussianFactorGraph recursively, from a full bayes tree.
*
* @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors
*/
template <class BAYESTREE>
GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) {
GaussianFactorGraph result;
BOOST_FOREACH(const typename BAYESTREE::sharedClique& root, bayesTree.roots())
result.push_back(liquefy<BAYESTREE>(root, splitConditionals));
return result;
}
} // \namespace gtsam

209
gtsam_unstable/linear/conditioning.cpp
View File

@ -1,209 +0,0 @@
/**
* @file summarization.cpp
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#include <gtsam_unstable/linear/conditioning.h>
#include <gtsam_unstable/linear/bayesTreeOperations.h>
#include <boost/assign/std/vector.hpp>
using namespace std;
using namespace boost::assign;
namespace gtsam {
/* ************************************************************************* */
GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shared_ptr& initConditional,
const std::set<Key>& saved_indices, const VectorValues& solution) {
const bool verbose = false;
if (!initConditional)
return initConditional;
if (verbose) {
cout << "backsubSummarize() Starting" << endl;
initConditional->print("Full initial conditional");
}
// Check for presence of variables to remove
std::set<Key> frontalsToRemove, parentsToRemove;
BOOST_FOREACH(const Key& frontal, initConditional->frontals())
if (!saved_indices.count(frontal))
frontalsToRemove.insert(frontal);
BOOST_FOREACH(const Key& parent, initConditional->parents())
if (!saved_indices.count(parent))
parentsToRemove.insert(parent);
// If all variables in this conditional are to be saved, just return initial conditional
if (frontalsToRemove.empty() && parentsToRemove.empty())
return initConditional;
// If none of the frontal variables are to be saved, return empty pointer
if (frontalsToRemove.size() == initConditional->nrFrontals())
return GaussianConditional::shared_ptr();
// Collect dimensions of the new conditional
if (verbose) cout << " Collecting dimensions" << endl;
size_t newTotalRows = 0, newTotalCols = 1; // Need spacing for RHS
size_t newNrFrontals = 0;
size_t oldOffset = 0;
vector<size_t> newDims, oldDims;
vector<size_t> oldColOffsets;
vector<Key> newIndices;
vector<size_t> newIdxToOldIdx; // Access to arrays, maps from new var to old var
const vector<Key>& oldIndices = initConditional->keys();
const size_t oldNrFrontals = initConditional->nrFrontals();
GaussianConditional::const_iterator varIt = initConditional->beginFrontals();
size_t oldIdx = 0;
for (; varIt != initConditional->endFrontals(); ++varIt) {
size_t varDim = initConditional->getDim(varIt);
oldDims += varDim;
if (!frontalsToRemove.count(*varIt)) {
newTotalCols += varDim;
newTotalRows += varDim;
newDims += varDim;
newIndices += *varIt;
++newNrFrontals;
newIdxToOldIdx += oldIdx;
oldColOffsets += oldOffset;
}
oldOffset += varDim;
++oldIdx;
}
varIt = initConditional->beginParents();
for (; varIt != initConditional->endParents(); ++varIt) {
size_t varDim = initConditional->getDim(varIt);
oldDims += varDim;
if (!parentsToRemove.count(*varIt)) {
newTotalCols += varDim;
newDims += varDim;
newIndices += *varIt;
}
}
newDims += 1; // For the RHS
// Initialize new conditional
Matrix full_matrix = Matrix::Zero(newTotalRows, newTotalCols);
Vector sigmas = zero(newTotalRows);
if (verbose) cout << " Initializing new conditional\nfull_matrix:\n" << full_matrix << endl;
// Fill in full matrix - iterate over rows for each sub-conditional
const size_t oldRNrCols = initConditional->get_R().cols();
size_t newColOffset = 0;
for (size_t newfrontalIdx=0; newfrontalIdx<newNrFrontals; ++newfrontalIdx) {
const size_t& dim = newDims.at(newfrontalIdx);
if (verbose) cout << " Filling in Matrix: newfrontalIdx " << newfrontalIdx
<< " frontalKey: " << newIndices[newfrontalIdx] << " dim: " << dim << endl;
size_t oldColOffset = oldColOffsets.at(newfrontalIdx);
// get R block, sliced by row
Eigen::Block<GaussianConditional::constABlock> rblock =
initConditional->get_R().block(oldColOffset, oldColOffset, dim, oldRNrCols-oldColOffset);
if (verbose) cout << " rblock\n" << rblock << endl;
// set the R matrix for this var
full_matrix.block(newColOffset, newColOffset, dim, dim) = rblock.leftCols(dim);
if (verbose) cout << " full_matrix: set R\n" << full_matrix << endl;
// set RHS
full_matrix.block(newColOffset, newTotalCols-1, dim, 1) = initConditional->get_d().segment(oldColOffset, dim);
if (verbose) cout << " full_matrix: set rhs\n" << full_matrix << endl;
// set sigmas
sigmas.segment(newColOffset, dim) = initConditional->get_model()->sigmas().segment(oldColOffset, dim);
// add parents in R block, while updating rhs
// Loop through old variable list
size_t newParentStartCol = newColOffset + dim;
size_t oldParentStartCol = dim; // Copying from Rblock - offset already accounted for
for (size_t oldIdx = newIdxToOldIdx[newfrontalIdx]+1; oldIdx<oldNrFrontals; ++oldIdx) {
Key parentKey = oldIndices[oldIdx];
size_t parentDim = oldDims[oldIdx];
if (verbose) cout << " Adding parents from R: parentKey: " << parentKey << " parentDim: " << parentDim << endl;
if (!frontalsToRemove.count(parentKey)) {
if (verbose) {
cout << " Copying block (from): oldParentStartCol " << oldParentStartCol << endl;
cout << " Copying block (to): newColOffset " << newColOffset << ", newParentStartCol " << newParentStartCol << endl;
}
full_matrix.block(newColOffset, newParentStartCol, dim, parentDim)
= rblock.middleCols(oldParentStartCol, parentDim);
newParentStartCol += parentDim;
if (verbose) cout << " full_matrix: add parent from R\n" << full_matrix << endl;
} else {
const Vector& parentSol = solution.at(parentKey);
full_matrix.block(newColOffset, newTotalCols-1, dim, 1) -=
rblock.middleCols(oldParentStartCol, parentDim) * parentSol;
if (verbose) cout << " full_matrix: update rhs from parent in R\n" << full_matrix << endl;
}
oldParentStartCol += parentDim;
}
// add parents (looping over original block structure), while updating rhs
GaussianConditional::const_iterator oldParent = initConditional->beginParents();
for (; oldParent != initConditional->endParents(); ++oldParent) {
Key parentKey = *oldParent;
size_t parentDim = initConditional->getDim(oldParent);
if (verbose) cout << " Adding parents from S: parentKey: " << parentKey << " parentDim: " << parentDim << endl;
if (parentsToRemove.count(parentKey)) {
// Solve out the variable
const Vector& parentSol = solution.at(parentKey);
assert((size_t)parentSol.size() == parentDim);
full_matrix.block(newColOffset, newTotalCols-1, dim, 1) -=
initConditional->get_S(oldParent).middleRows(oldColOffset, dim) * parentSol;
if (verbose) cout << " full_matrix: update rhs from parent in S\n" << full_matrix << endl;
} else {
// Copy the matrix block
full_matrix.block(newColOffset, newParentStartCol, dim, parentDim) =
initConditional->get_S(oldParent).middleRows(oldColOffset, dim);
if (verbose) cout << " full_matrix: add parent from S\n" << full_matrix << endl;
}
}
// Increment the rows
newColOffset += dim;
oldColOffset += dim;
}
// Construct a new conditional
if (verbose) cout << "backsubSummarize() Complete!" << endl;
// GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end());
VerticalBlockMatrix matrices(newDims, full_matrix);
return GaussianConditional::shared_ptr(new
GaussianConditional(newIndices, newNrFrontals, matrices, noiseModel::Diagonal::Sigmas(sigmas)));
}
/* ************************************************************************* */
GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree,
const std::set<Key>& saved_indices) {
const bool verbose = false;
VectorValues solution = bayesTree.optimize();
// FIXME: set of conditionals does not manage possibility of solving out whole separators
std::set<GaussianConditional::shared_ptr> affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices);
// Summarize conditionals separately
GaussianFactorGraph summarized_graph;
BOOST_FOREACH(const GaussianConditional::shared_ptr& conditional, affected_cliques) {
if (verbose) conditional->print("Initial conditional");
GaussianConditional::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution);
if (reducedConditional) {
if (verbose) reducedConditional->print("Final conditional");
summarized_graph.push_back(reducedConditional);
} else if (verbose) {
cout << "Conditional removed after summarization!" << endl;
}
}
return summarized_graph;
}
/* ************************************************************************* */
} // \namespace gtsam

45
gtsam_unstable/linear/conditioning.h
View File

@ -1,45 +0,0 @@
/**
* @file summarization.h
*
* @brief Types and utility functions for summarization
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#pragma once
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>
namespace gtsam {
/**
* Backsubstitution-based conditioning - reduces conditionals to
* densities on a sub-set of variables.
*
* Corner cases:
* - If no frontal vars are saved, returns a null pointer
*
* @param initConditional the conditional from which to remove a variable
* @param saved_indices is the set of indices that should appear in the result
* @param solution is a full solution for the system
*/
GTSAM_UNSTABLE_EXPORT GaussianConditional::shared_ptr conditionDensity(
const GaussianConditional::shared_ptr& initConditional,
const std::set<Key>& saved_indices, const VectorValues& solution);
/**
* Backsubstitution-based conditioning for a complete Bayes Tree - reduces
* conditionals by solving out variables to eliminate. Traverses the tree to
* add the correct dummy factors whenever a separator is eliminated.
*/
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph conditionDensity(
const GaussianBayesTree& bayesTree,
const std::set<Key>& saved_indices);
} // \namespace gtsam

56
gtsam_unstable/linear/corruptInitialization.h
View File

@ -1,56 +0,0 @@
/**
* @file corruptInitialization.h
*
* @brief Utilities for using noisemodels to corrupt given initialization value
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/linear/Sampler.h>
namespace gtsam {
/** given a noisemodel and a measurement, add noise to the measurement */
template<typename T>
T corruptWithNoise(const T& init,
const noiseModel::Base::shared_ptr& model, Sampler& sampler) {
Vector n = zero(model->dim());
noiseModel::Diagonal::shared_ptr
diag_model = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (diag_model)
n = sampler.sampleNewModel(diag_model);
return init.retract(n);
}
// specialization for doubles - just adds, rather than retract
template<>
inline double corruptWithNoise<double>(const double& init,
const noiseModel::Base::shared_ptr& model, Sampler& sampler) {
double n = 0.0;
noiseModel::Diagonal::shared_ptr
diag_model = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (diag_model)
n = sampler.sampleNewModel(diag_model)(0);
return init + n;
}
// specialization for doubles - just adds, rather than retract
template<>
inline Vector corruptWithNoise<Vector>(const Vector& init,
const noiseModel::Base::shared_ptr& model, Sampler& sampler) {
Vector n = zero(init.size());
noiseModel::Diagonal::shared_ptr
diag_model = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (diag_model)
n = sampler.sampleNewModel(diag_model);
return init + n;
}
} // \namespace gtsam

373
gtsam_unstable/linear/tests/testBayesTreeOperations.cpp
View File

@ -1,373 +0,0 @@
/**
* @file testLinearTools.cpp
*
* @brief
*
* @date Aug 27, 2012
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/linear/bayesTreeOperations.h>
#include <tests/smallExample.h>
using namespace gtsam;
SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(ones(2));
SharedDiagonal model4 = noiseModel::Diagonal::Sigmas(ones(4));
SharedDiagonal model6 = noiseModel::Diagonal::Sigmas(ones(6));
using namespace std;
using symbol_shorthand::X;
using symbol_shorthand::L;
static const double tol = 1e-4;
/* ************************************************************************* */
TEST( testBayesTreeOperations, splitFactor1 ) {
// Build upper-triangular system
JacobianFactor initFactor(
0,Matrix_(4, 2,
1.0, 2.0,
0.0, 3.0,
0.0, 0.0,
0.0, 0.0),
1,Matrix_(4, 2,
1.0, 2.0,
9.0, 3.0,
6.0, 8.0,
0.0, 7.0),
Vector_(4, 0.1, 0.2, 0.3, 0.4),
model4);
GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
GaussianFactorGraph expSplit;
expSplit.add(
0,Matrix_(2, 2,
1.0, 2.0,
0.0, 3.0),
1,Matrix_(2, 2,
1.0, 2.0,
9.0, 3.0),
Vector_(2, 0.1, 0.2),
model2);
expSplit.add(
1,Matrix_(2, 2,
6.0, 8.0,
0.0, 7.0),
Vector_(2, 0.3, 0.4),
model2);
EXPECT(assert_equal(expSplit, actSplit));
}
/* ************************************************************************* */
TEST( testBayesTreeOperations, splitFactor2 ) {
// Build upper-triangular system
JacobianFactor initFactor(
0,Matrix_(6, 2,
1.0, 2.0,
0.0, 3.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0),
1,Matrix_(6, 2,
1.0, 2.0,
9.0, 3.0,
6.0, 8.0,
0.0, 7.0,
0.0, 0.0,
0.0, 0.0),
2,Matrix_(6, 2,
1.1, 2.2,
9.1, 3.2,
6.1, 8.2,
0.1, 7.2,
0.1, 3.2,
0.0, 1.2),
Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6),
model6);
GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
GaussianFactorGraph expSplit;
expSplit.add(
0,Matrix_(2, 2,
1.0, 2.0,
0.0, 3.0),
1,Matrix_(2, 2,
1.0, 2.0,
9.0, 3.0),
2,Matrix_(2, 2,
1.1, 2.2,
9.1, 3.2),
Vector_(2, 0.1, 0.2),
model2);
expSplit.add(
1,Matrix_(2, 2,
6.0, 8.0,
0.0, 7.0),
2,Matrix_(2, 2,
6.1, 8.2,
0.1, 7.2),
Vector_(2, 0.3, 0.4),
model2);
expSplit.add(
2,Matrix_(2, 2,
0.1, 3.2,
0.0, 1.2),
Vector_(2, 0.5, 0.6),
model2);
EXPECT(assert_equal(expSplit, actSplit));
}
/* ************************************************************************* */
TEST( testBayesTreeOperations, splitFactor3 ) {
// Build upper-triangular system
JacobianFactor initFactor(
0,Matrix_(4, 2,
1.0, 2.0,
0.0, 3.0,
0.0, 0.0,
0.0, 0.0),
1,Matrix_(4, 2,
1.0, 2.0,
9.0, 3.0,
6.0, 8.0,
0.0, 7.0),
2,Matrix_(4, 2,
1.1, 2.2,
9.1, 3.2,
6.1, 8.2,
0.1, 7.2),
Vector_(4, 0.1, 0.2, 0.3, 0.4),
model4);
GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
GaussianFactorGraph expSplit;
expSplit.add(
0,Matrix_(2, 2,
1.0, 2.0,
0.0, 3.0),
1,Matrix_(2, 2,
1.0, 2.0,
9.0, 3.0),
2,Matrix_(2, 2,
1.1, 2.2,
9.1, 3.2),
Vector_(2, 0.1, 0.2),
model2);
expSplit.add(
1,Matrix_(2, 2,
6.0, 8.0,
0.0, 7.0),
2,Matrix_(2, 2,
6.1, 8.2,
0.1, 7.2),
Vector_(2, 0.3, 0.4),
model2);
EXPECT(assert_equal(expSplit, actSplit));
}
/* ************************************************************************* */
// 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;
/* ************************************************************************* */
TEST( testBayesTreeOperations, liquefy ) {
using namespace example;
// Create smoother with 7 nodes
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph smoother = createSmoother(7);
// Create the Bayes tree
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
// bayesTree.print("Full tree");
SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6)));
SharedDiagonal unit4 = noiseModel::Diagonal::Sigmas(Vector_(ones(4)));
SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector_(ones(2)));
// Liquefy the tree back into a graph
{
GaussianFactorGraph actGraph = liquefy(bayesTree, false); // Doesn't split conditionals
GaussianFactorGraph expGraph;
Matrix A12 = Matrix_(6, 2,
1.73205081, 0.0,
0.0, 1.73205081,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0);
Matrix A15 = Matrix_(6, 2,
-0.577350269, 0.0,
0.0, -0.577350269,
1.47196014, 0.0,
0.0, 1.47196014,
0.0, 0.0,
0.0, 0.0);
Matrix A16 = Matrix_(6, 2,
-0.577350269, 0.0,
0.0, -0.577350269,
-0.226455407, 0.0,
0.0, -0.226455407,
1.49357599, 0.0,
0.0, 1.49357599);
expGraph.add(2, A12, 5, A15, 6, A16, zeros(6,1), unit6);
Matrix A21 = Matrix_(4, 2,
1.73205081, 0.0,
0.0, 1.73205081,
0.0, 0.0,
0.0, 0.0);
Matrix A24 = Matrix_(4, 2,
-0.577350269, 0.0,
0.0, -0.577350269,
1.47196014, 0.0,
0.0, 1.47196014);
Matrix A26 = Matrix_(4, 2,
-0.577350269, 0.0,
0.0, -0.577350269,
-0.226455407, 0.0,
0.0, -0.226455407);
expGraph.add(1, A21, 4, A24, 6, A26, zeros(4,1), unit4);
Matrix A30 = Matrix_(2, 2,
1.41421356, 0.0,
0.0, 1.41421356);
Matrix A34 = Matrix_(2, 2,
-0.707106781, 0.0,
0.0, -0.707106781);
expGraph.add(0, A30, 4, A34, zeros(2,1), unit2);
Matrix A43 = Matrix_(2, 2,
1.41421356, 0.0,
0.0, 1.41421356);
Matrix A45 = Matrix_(2, 2,
-0.707106781, 0.0,
0.0, -0.707106781);
expGraph.add(3, A43, 5, A45, zeros(2,1), unit2);
EXPECT(assert_equal(expGraph, actGraph, tol));
}
// Liquefy the tree back into a graph, splitting factors
{
GaussianFactorGraph actGraph = liquefy(bayesTree, true);
GaussianFactorGraph expGraph;
// Conditional 1
{
Matrix A12 = Matrix_(2, 2,
1.73205081, 0.0,
0.0, 1.73205081);
Matrix A15 = Matrix_(2, 2,
-0.577350269, 0.0,
0.0, -0.577350269);
Matrix A16 = Matrix_(2, 2,
-0.577350269, 0.0,
0.0, -0.577350269);
expGraph.add(2, A12, 5, A15, 6, A16, zeros(2,1), unit2);
}
{
Matrix A15 = Matrix_(2, 2,
1.47196014, 0.0,
0.0, 1.47196014);
Matrix A16 = Matrix_(2, 2,
-0.226455407, 0.0,
0.0, -0.226455407);
expGraph.add(5, A15, 6, A16, zeros(2,1), unit2);
}
{
Matrix A16 = Matrix_(2, 2,
1.49357599, 0.0,
0.0, 1.49357599);
expGraph.add(6, A16, zeros(2,1), unit2);
}
// Conditional 2
{
Matrix A21 = Matrix_(2, 2,
1.73205081, 0.0,
0.0, 1.73205081);
Matrix A24 = Matrix_(2, 2,
-0.577350269, 0.0,
0.0, -0.577350269);
Matrix A26 = Matrix_(2, 2,
-0.577350269, 0.0,
0.0, -0.577350269);
expGraph.add(1, A21, 4, A24, 6, A26, zeros(2,1), unit2);
}
{
Matrix A24 = Matrix_(2, 2,
1.47196014, 0.0,
0.0, 1.47196014);
Matrix A26 = Matrix_(2, 2,
-0.226455407, 0.0,
0.0, -0.226455407);
expGraph.add(4, A24, 6, A26, zeros(2,1), unit2);
}
// Conditional 3
Matrix A30 = Matrix_(2, 2,
1.41421356, 0.0,
0.0, 1.41421356);
Matrix A34 = Matrix_(2, 2,
-0.707106781, 0.0,
0.0, -0.707106781);
expGraph.add(0, A30, 4, A34, zeros(2,1), unit2);
// Conditional 4
Matrix A43 = Matrix_(2, 2,
1.41421356, 0.0,
0.0, 1.41421356);
Matrix A45 = Matrix_(2, 2,
-0.707106781, 0.0,
0.0, -0.707106781);
expGraph.add(3, A43, 5, A45, zeros(2,1), unit2);
EXPECT(assert_equal(expGraph, actGraph, tol));
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

254
gtsam_unstable/linear/tests/testConditioning.cpp
View File

@ -1,254 +0,0 @@
/**
* @file testConditioning.cpp
*
* @brief Experiments using backsubstitution for conditioning (not summarization, it turns out)
*
* @date Sep 3, 2012
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/assign/std/set.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp>
#include <gtsam_unstable/linear/conditioning.h>
using namespace std;
using namespace boost::assign;
using namespace gtsam;
const double tol = 1e-5;
// Simple example
Matrix R = Matrix_(3,3,
1.0,-2.0,-3.0,
0.0, 3.0,-5.0,
0.0, 0.0, 6.0);
Vector d = Vector_(3,
0.1, 0.2, 0.3);
Vector x = Vector_(3,
0.55,
0.15,
0.05);
/* ************************************************************************* */
TEST( testConditioning, directed_elimination_example ) {
// create a 3-variable system from which to eliminate variables
// Scalar variables, pre-factorized into R,d system
// Use multifrontal representation
// Variables 0, 1, 2 - want to summarize out 1
Vector expx = R.triangularView<Eigen::Upper>().solve(d);
EXPECT(assert_equal(x, expx, tol));
EXPECT(assert_equal(Vector(R*x), d, tol));
// backsub-summarized version
Matrix Rprime = Matrix_(2,2,
1.0,-3.0,
0.0, 6.0);
Vector dprime = Vector_(2,
d(0) - R(0,1)*x(1),
d(2));
Vector xprime = Vector_(2,
x(0), // Same solution, just smaller
x(2));
EXPECT(assert_equal(Vector(Rprime*xprime), dprime, tol));
}
/* ************************************************************************* */
TEST( testConditioning, directed_elimination_singlefrontal ) {
// Gaussian conditional with a single frontal variable, parent is to be removed
// Top row from above example
Index root_key = 0, removed_key = 1, remaining_parent = 2;
Matrix R11 = Matrix_(1,1, 1.0), R22 = Matrix_(1,1, 3.0), S = Matrix_(1,1,-2.0), T = Matrix_(1,1,-3.0);
Vector d0 = d.segment(0,1), d1 = d.segment(1,1);
SharedDiagonal sigmas = noiseModel::Unit::Create(1);
GaussianConditional::shared_ptr initConditional(new
GaussianConditional(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas));
VectorValues solution;
solution.insert(0, x.segment(0,1));
solution.insert(1, x.segment(1,1));
solution.insert(2, x.segment(2,1));
std::set<Index> saved_indices;
saved_indices += root_key, remaining_parent;
GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
GaussianConditional::shared_ptr expSummarized(new
GaussianConditional(root_key, d0 - S*x(1), R11, remaining_parent, T, sigmas));
CHECK(actSummarized);
EXPECT(assert_equal(*expSummarized, *actSummarized, tol));
// Simple test of base case: if target index isn't present, return clone
GaussianConditional::shared_ptr actSummarizedSimple = conditionDensity(expSummarized, saved_indices, solution);
CHECK(actSummarizedSimple);
EXPECT(assert_equal(*expSummarized, *actSummarizedSimple, tol));
// case where frontal variable is to be eliminated - return null
GaussianConditional::shared_ptr removeFrontalInit(new
GaussianConditional(removed_key, d1, R22, remaining_parent, T, sigmas));
GaussianConditional::shared_ptr actRemoveFrontal = conditionDensity(removeFrontalInit, saved_indices, solution);
EXPECT(!actRemoveFrontal);
}
///* ************************************************************************* */
//TEST( testConditioning, directed_elimination_multifrontal ) {
// // Use top two rows from the previous example
// Index root_key = 0, removed_key = 1, remaining_parent = 2;
// Matrix R11 = R.topLeftCorner(2,2), S = R.block(0,2,2,1),
// Sprime = Matrix_(1,1,-2.0), R11prime = Matrix_(1,1, 1.0);
// Vector d1 = d.segment(0,2);
// SharedDiagonal sigmas1 = noiseModel::Unit::Create(1), sigmas2 = noiseModel::Unit::Create(2);
//
//
// std::list<std::pair<Index, Matrix> > terms;
// terms += make_pair(root_key, Matrix(R11.col(0)));
// terms += make_pair(removed_key, Matrix(R11.col(1)));
// terms += make_pair(remaining_parent, S);
// GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2));
//
// VectorValues solution;
// solution.insert(0, x.segment(0,1));
// solution.insert(1, x.segment(1,1));
// solution.insert(2, x.segment(2,1));
//
// std::set<Index> saved_indices;
// saved_indices += root_key, remaining_parent;
//
// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
// GaussianConditional::shared_ptr expSummarized(new
// GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1));
//
// CHECK(actSummarized);
// EXPECT(assert_equal(*expSummarized, *actSummarized, tol));
//}
//
///* ************************************************************************* */
//TEST( testConditioning, directed_elimination_multifrontal_multidim ) {
// // use larger example, three frontal variables, dim = 2 each, two parents (one removed)
// // Vars: 0, 1, 2, 3, 4; frontal: 0, 1, 2. parents: 3, 4;
// // Remove 1, 3
// Matrix Rinit = Matrix_(6, 11,
// 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.1,
// 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, 1.0, 0.2,
// 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,-1.0, 1.0, 0.0, 0.3,
// 0.0, 0.0, 0.0, 4.0, 0.0, 4.0, 3.0, 2.0, 0.0, 9.0, 0.4,
// 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 3.0, 0.0, 0.5,
// 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6);
//
// vector<size_t> init_dims; init_dims += 2, 2, 2, 2, 2, 1;
// VerticalBlockMatrix init_matrices(init_dims, Rinit);
// SharedDiagonal sigmas = noiseModel::Unit::Create(6);
// vector<size_t> init_keys; init_keys += 0, 1, 2, 3, 4;
// GaussianConditional::shared_ptr initConditional(new
// GaussianConditional(init_keys, 3, init_matrices, sigmas));
//
// // Construct a solution vector
// VectorValues solution;
// solution.insert(0, zero(2));
// solution.insert(1, zero(2));
// solution.insert(2, zero(2));
// solution.insert(3, Vector_(2, 1.0, 2.0));
// solution.insert(4, Vector_(2, 3.0, 4.0));
//
// solution = initConditional->solve(solution);
//
// std::set<Index> saved_indices;
// saved_indices += 0, 2, 4;
//
// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
// CHECK(actSummarized);
//
// Matrix Rexp = Matrix_(4, 7,
// 1.0, 0.0, 3.0, 0.0, -1.0, 0.0, 0.1,
// 0.0, 1.0, 0.0, 3.0, 0.0, 1.0, 0.2,
// 0.0, 0.0, 5.0, 0.0, 3.0, 0.0, 0.5,
// 0.0, 0.0, 0.0, 4.0, 0.0, 6.0, 0.6);
//
// // Update rhs
// Rexp.block(0, 6, 2, 1) -= Rinit.block(0, 2, 2, 2) * solution.at(1) + Rinit.block(0, 6, 2, 2) * solution.at(3);
// Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3);
//
// vector<size_t> exp_dims; exp_dims += 2, 2, 2, 1;
// VerticalBlockMatrix exp_matrices(exp_dims, Rexp);
// SharedDiagonal exp_sigmas = noiseModel::Unit::Create(4);
// vector<size_t> exp_keys; exp_keys += 0, 2, 4;
// GaussianConditional expSummarized(exp_keys, 2, exp_matrices, exp_sigmas);
//
// EXPECT(assert_equal(expSummarized, *actSummarized, tol));
//}
//
///* ************************************************************************* */
//TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) {
// // Example from LinearAugmentedSystem
// // 4 variables, last two in ordering kept - should be able to do this with no computation.
//
// vector<size_t> init_dims; init_dims += 3, 3, 2, 2, 1;
//
// //Full initial conditional: density on [3] [4] [5] [6]
// Matrix Rinit = Matrix_(10, 11,
// 8.78422312, -0.0375455118, -0.0387376278, -5.059576, 0.0, 0.0, -0.0887200041, 0.00429643583, -0.130078263, 0.0193260727, 0.0,
// 0.0, 8.46951839, 9.51456887, -0.0224291821, -5.24757636, 0.0, 0.0586258904, -0.173455825, 0.11090295, -0.330696013, 0.0,
// 0.0, 0.0, 16.5539485, 0.00105159359, -2.35354497, -6.04085484, -0.0212095105, 0.0978729072, 0.00471054272, 0.0694956367, 0.0,
// 0.0, 0.0, 0.0, 10.9015885, -0.0105694572, 0.000582715469, -0.0410535006, 0.00162772139, -0.0601433772, 0.0082824087,0.0,
// 0.0, 0.0, 0.0, 0.0, 10.5531086, -1.34722553, 0.02438072, -0.0644224578, 0.0561372492, -0.148932792, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 21.4870439, -0.00443305851, 0.0234766354, 0.00484572411, 0.0101997356, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.04823446, -0.302033014, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.24068986, 0.0);
// Vector dinit = Vector_(10,
// -0.00186915, 0.00318554, 0.000592421, -0.000861, 0.00171528, 0.000274123, -0.0284011, 0.0275465, 0.0439795, -0.0222134);
// Rinit.rightCols(1) = dinit;
// SharedDiagonal sigmas = noiseModel::Unit::Create(10);
//
// VerticalBlockMatrix init_matrices(init_dims, Rinit);
// vector<size_t> init_keys; init_keys += 3, 4, 5, 6;
// GaussianConditional::shared_ptr initConditional(new
// GaussianConditional(init_keys, 4, init_matrices, sigmas));
//
// // Calculate a solution
// VectorValues solution;
// solution.insert(0, zero(3));
// solution.insert(1, zero(3));
// solution.insert(2, zero(3));
// solution.insert(3, zero(3));
// solution.insert(4, zero(3));
// solution.insert(5, zero(2));
// solution.insert(6, zero(2));
//
// solution = initConditional->solve(solution);
//
// // Perform summarization
// std::set<Index> saved_indices;
// saved_indices += 5, 6;
//
// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
// CHECK(actSummarized);
//
// // Create expected value on [5], [6]
// Matrix Rexp = Matrix_(4, 5,
// 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, -0.0284011,
// 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0275465,
// 0.0, 0.0, 2.04823446, -0.302033014, 0.0439795,
// 0.0, 0.0, 0.0, 2.24068986, -0.0222134);
// SharedDiagonal expsigmas = noiseModel::Unit::Create(4);
//
// vector<size_t> exp_dims; exp_dims += 2, 2, 1;
// VerticalBlockMatrix exp_matrices(exp_dims, Rexp);
// vector<size_t> exp_keys; exp_keys += 5, 6;
// GaussianConditional expConditional(exp_keys, 2, exp_matrices, expsigmas);
//
// EXPECT(assert_equal(expConditional, *actSummarized, tol));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */