Merge remote-tracking branch 'svn/trunk' into windows
Conflicts: gtsam/linear/HessianFactor.h gtsam/nonlinear/Marginals.cpprelease/4.3a0
Richard Roberts
commit
1ca9e7049e
280
.cproject
280
.cproject
|
|
@ -593,6 +593,38 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testBTree.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testDSF.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testDSF.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testDSFVector.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testDSFVector.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testFixedVector.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testFixedVector.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -705,42 +737,34 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>testKey.run</buildTarget>
|
<buildTarget>tests/testGeneralSFMFactor.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>testGeneralSFMFactor.run</buildTarget>
|
<buildTarget>tests/testPlanarSLAM.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>testPlanarSLAM.run</buildTarget>
|
<buildTarget>tests/testPose2SLAM.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>testPose2SLAM.run</buildTarget>
|
<buildTarget>tests/testPose3SLAM.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j5</buildArguments>
|
|
||||||
<buildTarget>testPose3SLAM.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
@ -769,6 +793,46 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>schedulingExample.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testCSP.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testCSP.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testScheduler.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testScheduler.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="schedulingQuals12.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>schedulingQuals12.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testSudoku.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testSudoku.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -801,10 +865,10 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="check" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>check</buildTarget>
|
<buildTarget>testDiscreteFactor.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
@ -985,6 +1049,14 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="testNonlinearFactor.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -1600,82 +1672,74 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testVectorValues.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="linear.testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>linear.testNoiseModel.run</buildTarget>
|
<buildTarget>testVectorValues.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testGaussianFactor.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testHessianFactor.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testGaussianConditional.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testGaussianFactorGraph.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testGaussianJunctionTree.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testKalmanFilter.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testGaussianDensity.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="linear.testSerializationLinear.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>linear.testSerializationLinear.run</buildTarget>
|
<buildTarget>testNoiseModel.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testHessianFactor.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testGaussianConditional.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testGaussianFactorGraph.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testGaussianJunctionTree.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testKalmanFilter.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testGaussianDensity.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testSerializationLinear.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j5</buildArguments>
|
||||||
|
<buildTarget>testSerializationLinear.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
@ -1768,22 +1832,6 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="LocalizationExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j5</buildArguments>
|
|
||||||
<buildTarget>LocalizationExample.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="LocalizationExample2.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j5</buildArguments>
|
|
||||||
<buildTarget>LocalizationExample2.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -2147,18 +2195,18 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="wrap_gtsam_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check.discrete_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>wrap_gtsam_unstable</buildTarget>
|
<buildTarget>check.discrete_unstable</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="check.wrap" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check.base_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>check.wrap</buildTarget>
|
<buildTarget>check.base_unstable</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
@ -2179,26 +2227,26 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="check.base_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check.unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>check.base_unstable</buildTarget>
|
<buildTarget>check.unstable</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>testSpirit.run</buildTarget>
|
<buildTarget>wrap.testSpirit.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="wrap.testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>testWrap.run</buildTarget>
|
<buildTarget>wrap.testWrap.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
|
||||||
|
|
@ -69,6 +69,9 @@ option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab
|
||||||
option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON)
|
option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON)
|
||||||
option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON)
|
option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON)
|
||||||
|
|
||||||
|
# Experimental - features disabled by default
|
||||||
|
option(GTSAM_ENABLE_BUILD_MEX_BINARIES "Enable/Disable building of matlab mex files" OFF)
|
||||||
|
|
||||||
# Flags for choosing default packaging tools
|
# Flags for choosing default packaging tools
|
||||||
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
|
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
|
||||||
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
|
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
|
||||||
|
|
|
||||||
|
|
@ -41,7 +41,7 @@ SimpleString::SimpleString (const SimpleString& other)
|
||||||
|
|
||||||
SimpleString SimpleString::operator= (const SimpleString& other)
|
SimpleString SimpleString::operator= (const SimpleString& other)
|
||||||
{
|
{
|
||||||
delete buffer_;
|
delete [] buffer_;
|
||||||
buffer_ = new char [other.size() + 1];
|
buffer_ = new char [other.size() + 1];
|
||||||
strcpy(buffer_, other.buffer_);
|
strcpy(buffer_, other.buffer_);
|
||||||
return *this;
|
return *this;
|
||||||
|
|
@ -50,7 +50,7 @@ SimpleString SimpleString::operator= (const SimpleString& other)
|
||||||
SimpleString SimpleString::operator+ (const SimpleString& other)
|
SimpleString SimpleString::operator+ (const SimpleString& other)
|
||||||
{
|
{
|
||||||
SimpleString ret;
|
SimpleString ret;
|
||||||
delete ret.buffer_;
|
delete [] ret.buffer_;
|
||||||
ret.buffer_ = new char [size() + other.size() - 1];
|
ret.buffer_ = new char [size() + other.size() - 1];
|
||||||
strcpy(ret.buffer_, buffer_);
|
strcpy(ret.buffer_, buffer_);
|
||||||
strcat(ret.buffer_, other.buffer_);
|
strcat(ret.buffer_, other.buffer_);
|
||||||
|
|
|
||||||
|
|
@ -113,7 +113,7 @@ int main(int argc, char** argv) {
|
||||||
|
|
||||||
// first using sequential elimination
|
// first using sequential elimination
|
||||||
LevenbergMarquardtParams lmParams;
|
LevenbergMarquardtParams lmParams;
|
||||||
lmParams.elimination = LevenbergMarquardtParams::SEQUENTIAL;
|
lmParams.linearSolverType = LevenbergMarquardtParams::SEQUENTIAL_CHOLESKY;
|
||||||
Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
|
Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
|
||||||
resultSequential.print("final result (solved with a sequential solver)");
|
resultSequential.print("final result (solved with a sequential solver)");
|
||||||
|
|
||||||
|
|
|
||||||
1
gtsam.h
1
gtsam.h
|
|
@ -489,7 +489,6 @@ class Values {
|
||||||
Values();
|
Values();
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
void insertPose(int key, const gtsam::Pose2& pose);
|
void insertPose(int key, const gtsam::Pose2& pose);
|
||||||
gtsam::Symbol poseKey(int i);
|
|
||||||
gtsam::Pose2 pose(int i);
|
gtsam::Pose2 pose(int i);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -49,7 +49,7 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void DecisionTreeFactor::print(const string& s) const {
|
void DecisionTreeFactor::print(const string& s) const {
|
||||||
cout << s << ":\n";
|
cout << s;
|
||||||
IndexFactor::print("IndexFactor:");
|
IndexFactor::print("IndexFactor:");
|
||||||
Potentials::print("Potentials:");
|
Potentials::print("Potentials:");
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -72,7 +72,7 @@ namespace gtsam {
|
||||||
bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const;
|
bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const;
|
||||||
|
|
||||||
// print
|
// print
|
||||||
void print(const std::string& s = "DecisionTreeFactor: ") const;
|
void print(const std::string& s = "DecisionTreeFactor:\n") const;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Standard Interface
|
/// @name Standard Interface
|
||||||
|
|
|
||||||
|
|
@ -82,7 +82,7 @@ namespace gtsam {
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
// print
|
// print
|
||||||
virtual void print(const std::string& s = "DiscreteFactor") const {
|
virtual void print(const std::string& s = "DiscreteFactor\n") const {
|
||||||
IndexFactor::print(s);
|
IndexFactor::print(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,326 +1,326 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
* See LICENSE for the license information
|
* See LICENSE for the license information
|
||||||
|
|
||||||
* -------------------------------------------------------------------------- */
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file HessianFactor.h
|
* @file HessianFactor.h
|
||||||
* @brief Contains the HessianFactor class, a general quadratic factor
|
* @brief Contains the HessianFactor class, a general quadratic factor
|
||||||
* @author Richard Roberts
|
* @author Richard Roberts
|
||||||
* @date Dec 8, 2010
|
* @date Dec 8, 2010
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/blockMatrices.h>
|
#include <gtsam/base/blockMatrices.h>
|
||||||
#include <gtsam/inference/FactorGraph.h>
|
#include <gtsam/inference/FactorGraph.h>
|
||||||
#include <gtsam/linear/GaussianFactor.h>
|
#include <gtsam/linear/GaussianFactor.h>
|
||||||
|
|
||||||
// Forward declarations for friend unit tests
|
// Forward declarations for friend unit tests
|
||||||
class ConversionConstructorHessianFactorTest;
|
class ConversionConstructorHessianFactorTest;
|
||||||
class Constructor1HessianFactorTest;
|
class Constructor1HessianFactorTest;
|
||||||
class Constructor1bHessianFactorTest;
|
class Constructor1bHessianFactorTest;
|
||||||
class combineHessianFactorTest;
|
class combineHessianFactorTest;
|
||||||
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
// Forward declarations
|
// Forward declarations
|
||||||
class JacobianFactor;
|
class JacobianFactor;
|
||||||
class SharedDiagonal;
|
class SharedDiagonal;
|
||||||
class GaussianConditional;
|
class GaussianConditional;
|
||||||
template<class C> class BayesNet;
|
template<class C> class BayesNet;
|
||||||
|
|
||||||
// Definition of Scatter, which is an intermediate data structure used when
|
// Definition of Scatter, which is an intermediate data structure used when
|
||||||
// building a HessianFactor incrementally, to get the keys in the right
|
// building a HessianFactor incrementally, to get the keys in the right
|
||||||
// order.
|
// order.
|
||||||
struct SlotEntry {
|
struct SlotEntry {
|
||||||
size_t slot;
|
size_t slot;
|
||||||
size_t dimension;
|
size_t dimension;
|
||||||
SlotEntry(size_t _slot, size_t _dimension)
|
SlotEntry(size_t _slot, size_t _dimension)
|
||||||
: slot(_slot), dimension(_dimension) {}
|
: slot(_slot), dimension(_dimension) {}
|
||||||
std::string toString() const;
|
std::string toString() const;
|
||||||
};
|
};
|
||||||
typedef FastMap<Index, SlotEntry> Scatter;
|
typedef FastMap<Index, SlotEntry> Scatter;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief A Gaussian factor using the canonical parameters (information form)
|
* @brief A Gaussian factor using the canonical parameters (information form)
|
||||||
*
|
*
|
||||||
* HessianFactor implements a general quadratic factor of the form
|
* HessianFactor implements a general quadratic factor of the form
|
||||||
* \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f]
|
* \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f]
|
||||||
* that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$.
|
* that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$.
|
||||||
*
|
*
|
||||||
* When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian,
|
* When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian,
|
||||||
* in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$,
|
* in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$,
|
||||||
* \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual
|
* \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual
|
||||||
* sum-square-error at the mean, when \f$ x = \mu \f$.
|
* sum-square-error at the mean, when \f$ x = \mu \f$.
|
||||||
*
|
*
|
||||||
* Indeed, the negative log-likelihood of a Gaussian is (up to a constant)
|
* Indeed, the negative log-likelihood of a Gaussian is (up to a constant)
|
||||||
* @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$
|
* @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$
|
||||||
* with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get
|
* with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get
|
||||||
* @f[
|
* @f[
|
||||||
* E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu
|
* E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu
|
||||||
* @f]
|
* @f]
|
||||||
* We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$
|
* We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$
|
||||||
* and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$
|
* and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$
|
||||||
* to arrive at the canonical form of the Gaussian:
|
* to arrive at the canonical form of the Gaussian:
|
||||||
* @f[
|
* @f[
|
||||||
* E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu
|
* E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu
|
||||||
* @f]
|
* @f]
|
||||||
*
|
*
|
||||||
* This factor is one of the factors that can be in a GaussianFactorGraph.
|
* This factor is one of the factors that can be in a GaussianFactorGraph.
|
||||||
* It may be returned from NonlinearFactor::linearize(), but is also
|
* It may be returned from NonlinearFactor::linearize(), but is also
|
||||||
* used internally to store the Hessian during Cholesky elimination.
|
* used internally to store the Hessian during Cholesky elimination.
|
||||||
*
|
*
|
||||||
* This can represent a quadratic factor with characteristics that cannot be
|
* This can represent a quadratic factor with characteristics that cannot be
|
||||||
* represented using a JacobianFactor (which has the form
|
* represented using a JacobianFactor (which has the form
|
||||||
* \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$
|
* \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$
|
||||||
* and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example,
|
* and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example,
|
||||||
* a HessianFactor need not be positive semidefinite, it can be indefinite or
|
* a HessianFactor need not be positive semidefinite, it can be indefinite or
|
||||||
* even negative semidefinite.
|
* even negative semidefinite.
|
||||||
*
|
*
|
||||||
* If a HessianFactor is indefinite or negative semi-definite, then in order
|
* If a HessianFactor is indefinite or negative semi-definite, then in order
|
||||||
* for solving the linear system to be possible,
|
* for solving the linear system to be possible,
|
||||||
* the Hessian of the full system must be positive definite (i.e. when all
|
* the Hessian of the full system must be positive definite (i.e. when all
|
||||||
* small Hessians are combined, the result must be positive definite). If
|
* small Hessians are combined, the result must be positive definite). If
|
||||||
* this is not the case, an error will occur during elimination.
|
* this is not the case, an error will occur during elimination.
|
||||||
*
|
*
|
||||||
* This class stores G, g, and f as an augmented matrix HessianFactor::matrix_.
|
* This class stores G, g, and f as an augmented matrix HessianFactor::matrix_.
|
||||||
* The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right
|
* The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right
|
||||||
* triangle of G, the upper-right-most column of length n of HessianFactor::matrix_
|
* triangle of G, the upper-right-most column of length n of HessianFactor::matrix_
|
||||||
* stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e.
|
* stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e.
|
||||||
* \code
|
* \code
|
||||||
HessianFactor::matrix_ = [ G11 G12 G13 ... g1
|
HessianFactor::matrix_ = [ G11 G12 G13 ... g1
|
||||||
0 G22 G23 ... g2
|
0 G22 G23 ... g2
|
||||||
0 0 G33 ... g3
|
0 0 G33 ... g3
|
||||||
: : : :
|
: : : :
|
||||||
0 0 0 ... f ]
|
0 0 0 ... f ]
|
||||||
\endcode
|
\endcode
|
||||||
Blocks can be accessed as follows:
|
Blocks can be accessed as follows:
|
||||||
\code
|
\code
|
||||||
G11 = info(begin(), begin());
|
G11 = info(begin(), begin());
|
||||||
G12 = info(begin(), begin()+1);
|
G12 = info(begin(), begin()+1);
|
||||||
G23 = info(begin()+1, begin()+2);
|
G23 = info(begin()+1, begin()+2);
|
||||||
g2 = linearTerm(begin()+1);
|
g2 = linearTerm(begin()+1);
|
||||||
f = constantTerm();
|
f = constantTerm();
|
||||||
.......
|
.......
|
||||||
\endcode
|
\endcode
|
||||||
*/
|
*/
|
||||||
class HessianFactor : public GaussianFactor {
|
class HessianFactor : public GaussianFactor {
|
||||||
protected:
|
protected:
|
||||||
typedef Matrix InfoMatrix; ///< The full augmented Hessian
|
typedef Matrix InfoMatrix; ///< The full augmented Hessian
|
||||||
typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
|
typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
|
||||||
typedef GaussianFactor Base; ///< Typedef to base class
|
typedef GaussianFactor Base; ///< Typedef to base class
|
||||||
|
|
||||||
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
|
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.
|
BlockInfo info_; ///< The block view of the full information matrix.
|
||||||
|
|
||||||
/** Update the factor by adding the information from the JacobianFactor
|
/** Update the factor by adding the information from the JacobianFactor
|
||||||
* (used internally during elimination).
|
* (used internally during elimination).
|
||||||
* @param update The JacobianFactor containing the new information to add
|
* @param update The JacobianFactor containing the new information to add
|
||||||
* @param scatter A mapping from variable index to slot index in this HessianFactor
|
* @param scatter A mapping from variable index to slot index in this HessianFactor
|
||||||
*/
|
*/
|
||||||
void updateATA(const JacobianFactor& update, const Scatter& scatter);
|
void updateATA(const JacobianFactor& update, const Scatter& scatter);
|
||||||
|
|
||||||
/** Update the factor by adding the information from the HessianFactor
|
/** Update the factor by adding the information from the HessianFactor
|
||||||
* (used internally during elimination).
|
* (used internally during elimination).
|
||||||
* @param update The HessianFactor containing the new information to add
|
* @param update The HessianFactor containing the new information to add
|
||||||
* @param scatter A mapping from variable index to slot index in this HessianFactor
|
* @param scatter A mapping from variable index to slot index in this HessianFactor
|
||||||
*/
|
*/
|
||||||
void updateATA(const HessianFactor& update, const Scatter& scatter);
|
void updateATA(const HessianFactor& update, const Scatter& scatter);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
|
typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
|
||||||
typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
|
typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
|
||||||
typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
|
typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
|
||||||
typedef BlockInfo::Column Column; ///< A column containing the linear term h
|
typedef BlockInfo::Column Column; ///< A column containing the linear term h
|
||||||
typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
|
typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
|
||||||
|
|
||||||
/** Copy constructor */
|
/** Copy constructor */
|
||||||
HessianFactor(const HessianFactor& gf);
|
HessianFactor(const HessianFactor& gf);
|
||||||
|
|
||||||
/** default constructor for I/O */
|
/** default constructor for I/O */
|
||||||
HessianFactor();
|
HessianFactor();
|
||||||
|
|
||||||
/** Construct a unary factor. G is the quadratic term (Hessian matrix), g
|
/** Construct a unary factor. G is the quadratic term (Hessian matrix), g
|
||||||
* the linear term (a vector), and f the constant term. The quadratic
|
* the linear term (a vector), and f the constant term. The quadratic
|
||||||
* error is:
|
* error is:
|
||||||
* 0.5*(f - 2*x'*g + x'*G*x)
|
* 0.5*(f - 2*x'*g + x'*G*x)
|
||||||
*/
|
*/
|
||||||
HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
|
HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
|
||||||
|
|
||||||
/** Construct a unary factor, given a mean and covariance matrix.
|
/** Construct a unary factor, given a mean and covariance matrix.
|
||||||
* error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
|
* error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
|
||||||
*/
|
*/
|
||||||
HessianFactor(Index j, const Vector& mu, const Matrix& Sigma);
|
HessianFactor(Index j, const Vector& mu, const Matrix& Sigma);
|
||||||
|
|
||||||
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
|
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
|
||||||
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
|
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
|
||||||
* term, and f the constant term.
|
* term, and f the constant term.
|
||||||
* JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
|
* JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
|
||||||
* HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f]
|
* HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f]
|
||||||
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
|
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
|
||||||
\code
|
\code
|
||||||
n1*n1 G11 = A1'*M*A1
|
n1*n1 G11 = A1'*M*A1
|
||||||
n1*n2 G12 = A1'*M*A2
|
n1*n2 G12 = A1'*M*A2
|
||||||
n2*n2 G22 = A2'*M*A2
|
n2*n2 G22 = A2'*M*A2
|
||||||
n1*1 g1 = A1'*M*b
|
n1*1 g1 = A1'*M*b
|
||||||
n2*1 g2 = A2'*M*b
|
n2*1 g2 = A2'*M*b
|
||||||
1*1 f = b'*M*b
|
1*1 f = b'*M*b
|
||||||
\endcode
|
\endcode
|
||||||
*/
|
*/
|
||||||
HessianFactor(Index j1, Index j2,
|
HessianFactor(Index j1, Index j2,
|
||||||
const Matrix& G11, const Matrix& G12, const Vector& g1,
|
const Matrix& G11, const Matrix& G12, const Vector& g1,
|
||||||
const Matrix& G22, const Vector& g2, double f);
|
const Matrix& G22, const Vector& g2, double f);
|
||||||
|
|
||||||
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
|
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
|
||||||
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
|
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
|
||||||
* term, and f the constant term.
|
* term, and f the constant term.
|
||||||
*/
|
*/
|
||||||
HessianFactor(Index j1, Index j2, Index j3,
|
HessianFactor(Index j1, Index j2, Index j3,
|
||||||
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
|
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
|
||||||
const Matrix& G22, const Matrix& G23, const Vector& g2,
|
const Matrix& G22, const Matrix& G23, const Vector& g2,
|
||||||
const Matrix& G33, const Vector& g3, double f);
|
const Matrix& G33, const Vector& g3, double f);
|
||||||
|
|
||||||
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
|
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
|
||||||
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
|
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
|
||||||
* of the linear vector term, and f the constant term.
|
* of the linear vector term, and f the constant term.
|
||||||
*/
|
*/
|
||||||
HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
|
HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
|
||||||
const std::vector<Vector>& gs, double f);
|
const std::vector<Vector>& gs, double f);
|
||||||
|
|
||||||
/** Construct from Conditional Gaussian */
|
/** Construct from Conditional Gaussian */
|
||||||
HessianFactor(const GaussianConditional& cg);
|
HessianFactor(const GaussianConditional& cg);
|
||||||
|
|
||||||
/** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
|
/** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
|
||||||
HessianFactor(const GaussianFactor& factor);
|
HessianFactor(const GaussianFactor& factor);
|
||||||
|
|
||||||
/** Special constructor used in EliminateCholesky which combines the given factors */
|
/** Special constructor used in EliminateCholesky which combines the given factors */
|
||||||
HessianFactor(const FactorGraph<GaussianFactor>& factors,
|
HessianFactor(const FactorGraph<GaussianFactor>& factors,
|
||||||
const std::vector<size_t>& dimensions, const Scatter& scatter);
|
const std::vector<size_t>& dimensions, const Scatter& scatter);
|
||||||
|
|
||||||
/** Destructor */
|
/** Destructor */
|
||||||
virtual ~HessianFactor() {}
|
virtual ~HessianFactor() {}
|
||||||
|
|
||||||
/** Aassignment operator */
|
/** Aassignment operator */
|
||||||
HessianFactor& operator=(const HessianFactor& rhs);
|
HessianFactor& operator=(const HessianFactor& rhs);
|
||||||
|
|
||||||
/** Clone this JacobianFactor */
|
/** Clone this JacobianFactor */
|
||||||
virtual GaussianFactor::shared_ptr clone() const {
|
virtual GaussianFactor::shared_ptr clone() const {
|
||||||
return boost::static_pointer_cast<GaussianFactor>(
|
return boost::static_pointer_cast<GaussianFactor>(
|
||||||
shared_ptr(new HessianFactor(*this)));
|
shared_ptr(new HessianFactor(*this)));
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Print the factor for debugging and testing (implementing Testable) */
|
/** Print the factor for debugging and testing (implementing Testable) */
|
||||||
virtual void print(const std::string& s = "") const;
|
virtual void print(const std::string& s = "") const;
|
||||||
|
|
||||||
/** Compare to another factor for testing (implementing Testable) */
|
/** Compare to another factor for testing (implementing Testable) */
|
||||||
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
|
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
|
||||||
|
|
||||||
/** Evaluate the factor error f(x), see above. */
|
/** Evaluate the factor error f(x), see above. */
|
||||||
virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */
|
virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */
|
||||||
|
|
||||||
/** Return the dimension of the variable pointed to by the given key iterator
|
/** Return the dimension of the variable pointed to by the given key iterator
|
||||||
* todo: Remove this in favor of keeping track of dimensions with variables?
|
* todo: Remove this in favor of keeping track of dimensions with variables?
|
||||||
* @param variable An iterator pointing to the slot in this factor. You can
|
* @param variable An iterator pointing to the slot in this factor. You can
|
||||||
* use, for example, begin() + 2 to get the 3rd variable in this factor.
|
* use, for example, begin() + 2 to get the 3rd variable in this factor.
|
||||||
*/
|
*/
|
||||||
virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); }
|
virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); }
|
||||||
|
|
||||||
/** Return the number of columns and rows of the Hessian matrix */
|
/** Return the number of columns and rows of the Hessian matrix */
|
||||||
size_t rows() const { return info_.rows(); }
|
size_t rows() const { return info_.rows(); }
|
||||||
|
|
||||||
/** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
|
/** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
|
||||||
* information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation
|
* 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
|
* above to explain that only the upper-triangular part of the information matrix is stored
|
||||||
* and returned by this function.
|
* and returned by this function.
|
||||||
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
|
* @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.
|
* 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
|
* @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.
|
* use, for example, begin() + 2 to get the 3rd variable in this factor.
|
||||||
* @return A view of the requested block, not a copy.
|
* @return A view of the requested block, not a copy.
|
||||||
*/
|
*/
|
||||||
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
|
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
|
||||||
|
|
||||||
/** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
|
/** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
|
||||||
* as described above. See HessianFactor class documentation above to explain that only the
|
* 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.
|
* upper-triangular part of the information matrix is stored and returned by this function.
|
||||||
*/
|
*/
|
||||||
constBlock info() const { return info_.full(); }
|
constBlock info() const { return info_.full(); }
|
||||||
|
|
||||||
/** Return the constant term \f$ f \f$ as described above
|
/** Return the constant term \f$ f \f$ as described above
|
||||||
* @return The constant term \f$ f \f$
|
* @return The constant term \f$ f \f$
|
||||||
*/
|
*/
|
||||||
double constantTerm() const;
|
double constantTerm() const;
|
||||||
|
|
||||||
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
|
/** 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
|
* @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.
|
* use, for example, begin() + 2 to get the 3rd variable in this factor.
|
||||||
* @return The linear term \f$ g \f$ */
|
* @return The linear term \f$ g \f$ */
|
||||||
constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
|
constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
|
||||||
|
|
||||||
/** Return the complete linear term \f$ g \f$ as described above.
|
/** Return the complete linear term \f$ g \f$ as described above.
|
||||||
* @return The linear term \f$ g \f$ */
|
* @return The linear term \f$ g \f$ */
|
||||||
constColumn linearTerm() const;
|
constColumn linearTerm() const;
|
||||||
|
|
||||||
/** Return the augmented information matrix represented by this GaussianFactor.
|
/** Return the augmented information matrix represented by this GaussianFactor.
|
||||||
* The augmented information matrix contains the information matrix with an
|
* The augmented information matrix contains the information matrix with an
|
||||||
* additional column holding the information vector, and an additional row
|
* additional column holding the information vector, and an additional row
|
||||||
* holding the transpose of the information vector. The lower-right entry
|
* holding the transpose of the information vector. The lower-right entry
|
||||||
* contains the constant error term (when \f$ \delta x = 0 \f$). The
|
* contains the constant error term (when \f$ \delta x = 0 \f$). The
|
||||||
* augmented information matrix is described in more detail in HessianFactor,
|
* augmented information matrix is described in more detail in HessianFactor,
|
||||||
* which in fact stores an augmented information matrix.
|
* which in fact stores an augmented information matrix.
|
||||||
*
|
*
|
||||||
* For HessianFactor, this is the same as info() except that this function
|
* For HessianFactor, this is the same as info() except that this function
|
||||||
* returns a complete symmetric matrix whereas info() returns a matrix where
|
* returns a complete symmetric matrix whereas info() returns a matrix where
|
||||||
* only the upper triangle is valid, but should be interpreted as symmetric.
|
* only the upper triangle is valid, but should be interpreted as symmetric.
|
||||||
* This is because info() returns only a reference to the internal
|
* This is because info() returns only a reference to the internal
|
||||||
* representation of the augmented information matrix, which stores only the
|
* representation of the augmented information matrix, which stores only the
|
||||||
* upper triangle.
|
* upper triangle.
|
||||||
*/
|
*/
|
||||||
virtual Matrix computeInformation() const;
|
virtual Matrix computeInformation() const;
|
||||||
|
|
||||||
// Friend unit test classes
|
// Friend unit test classes
|
||||||
friend class ::ConversionConstructorHessianFactorTest;
|
friend class ::ConversionConstructorHessianFactorTest;
|
||||||
friend class ::Constructor1HessianFactorTest;
|
friend class ::Constructor1HessianFactorTest;
|
||||||
friend class ::Constructor1bHessianFactorTest;
|
friend class ::Constructor1bHessianFactorTest;
|
||||||
friend class ::combineHessianFactorTest;
|
friend class ::combineHessianFactorTest;
|
||||||
|
|
||||||
// Friend JacobianFactor for conversion
|
// Friend JacobianFactor for conversion
|
||||||
friend class JacobianFactor;
|
friend class JacobianFactor;
|
||||||
|
|
||||||
// used in eliminateCholesky:
|
// used in eliminateCholesky:
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Do Cholesky. Note that after this, the lower triangle still contains
|
* Do Cholesky. Note that after this, the lower triangle still contains
|
||||||
* some untouched non-zeros that should be zero. We zero them while
|
* some untouched non-zeros that should be zero. We zero them while
|
||||||
* extracting submatrices in splitEliminatedFactor. Frank says :-(
|
* extracting submatrices in splitEliminatedFactor. Frank says :-(
|
||||||
*/
|
*/
|
||||||
void partialCholesky(size_t nrFrontals);
|
void partialCholesky(size_t nrFrontals);
|
||||||
|
|
||||||
/** split partially eliminated factor */
|
/** split partially eliminated factor */
|
||||||
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
|
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
|
||||||
|
|
||||||
/** assert invariants */
|
/** assert invariants */
|
||||||
void assertInvariants() const;
|
void assertInvariants() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
|
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
|
||||||
ar & BOOST_SERIALIZATION_NVP(info_);
|
ar & BOOST_SERIALIZATION_NVP(info_);
|
||||||
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -154,6 +154,16 @@ namespace gtsam {
|
||||||
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
|
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
|
||||||
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
|
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
|
||||||
virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
|
virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
|
||||||
|
|
||||||
|
/** Return the augmented information matrix represented by this GaussianFactor.
|
||||||
|
* The augmented information matrix contains the information matrix with an
|
||||||
|
* additional column holding the information vector, and an additional row
|
||||||
|
* holding the transpose of the information vector. The lower-right entry
|
||||||
|
* contains the constant error term (when \f$ \delta x = 0 \f$). The
|
||||||
|
* augmented information matrix is described in more detail in HessianFactor,
|
||||||
|
* which in fact stores an augmented information matrix.
|
||||||
|
*/
|
||||||
|
virtual Matrix computeInformation() const;
|
||||||
|
|
||||||
/** Return the augmented information matrix represented by this GaussianFactor.
|
/** Return the augmented information matrix represented by this GaussianFactor.
|
||||||
* The augmented information matrix contains the information matrix with an
|
* The augmented information matrix contains the information matrix with an
|
||||||
|
|
|
||||||
|
|
@ -33,25 +33,25 @@ void DoglegOptimizer::iterate(void) {
|
||||||
const Ordering& ordering = *params_.ordering;
|
const Ordering& ordering = *params_.ordering;
|
||||||
GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, ordering);
|
GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, ordering);
|
||||||
|
|
||||||
// Get elimination method
|
|
||||||
GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
|
|
||||||
|
|
||||||
// Pull out parameters we'll use
|
// Pull out parameters we'll use
|
||||||
const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT);
|
const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT);
|
||||||
|
|
||||||
// Do Dogleg iteration with either Multifrontal or Sequential elimination
|
// Do Dogleg iteration with either Multifrontal or Sequential elimination
|
||||||
DoglegOptimizerImpl::IterationResult result;
|
DoglegOptimizerImpl::IterationResult result;
|
||||||
|
|
||||||
if(params_.elimination == DoglegParams::MULTIFRONTAL) {
|
if ( params_.isMultifrontal() ) {
|
||||||
GaussianBayesTree bt;
|
GaussianBayesTree bt;
|
||||||
bt.insert(GaussianJunctionTree(*linear).eliminate(eliminationMethod));
|
bt.insert(GaussianJunctionTree(*linear).eliminate(params_.getEliminationFunction()));
|
||||||
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, ordering, state_.error, dlVerbose);
|
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, ordering, state_.error, dlVerbose);
|
||||||
|
}
|
||||||
} else if(params_.elimination == DoglegParams::SEQUENTIAL) {
|
else if ( params_.isSequential() ) {
|
||||||
GaussianBayesNet::shared_ptr bn = EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod);
|
GaussianBayesNet::shared_ptr bn = EliminationTree<GaussianFactor>::Create(*linear)->eliminate(params_.getEliminationFunction());
|
||||||
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *bn, graph_, state_.values, ordering, state_.error, dlVerbose);
|
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *bn, graph_, state_.values, ordering, state_.error, dlVerbose);
|
||||||
|
}
|
||||||
} else {
|
else if ( params_.isCG() ) {
|
||||||
|
throw runtime_error("todo: ");
|
||||||
|
}
|
||||||
|
else {
|
||||||
throw runtime_error("Optimization parameter is invalid: DoglegParams::elimination");
|
throw runtime_error("Optimization parameter is invalid: DoglegParams::elimination");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -35,13 +35,18 @@ void GaussNewtonOptimizer::iterate() {
|
||||||
// Optimize
|
// Optimize
|
||||||
VectorValues delta;
|
VectorValues delta;
|
||||||
{
|
{
|
||||||
GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
|
if ( params_.isMultifrontal() ) {
|
||||||
if(params_.elimination == GaussNewtonParams::MULTIFRONTAL)
|
delta = GaussianJunctionTree(*linear).optimize(params_.getEliminationFunction());
|
||||||
delta = GaussianJunctionTree(*linear).optimize(eliminationMethod);
|
}
|
||||||
else if(params_.elimination == GaussNewtonParams::SEQUENTIAL)
|
else if ( params_.isSequential() ) {
|
||||||
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod));
|
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(params_.getEliminationFunction()));
|
||||||
else
|
}
|
||||||
|
else if ( params_.isCG() ) {
|
||||||
|
throw runtime_error("todo: ");
|
||||||
|
}
|
||||||
|
else {
|
||||||
throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination");
|
throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Maybe show output
|
// Maybe show output
|
||||||
|
|
|
||||||
|
|
@ -490,6 +490,9 @@ public:
|
||||||
/** Access the current ordering */
|
/** Access the current ordering */
|
||||||
const Ordering& getOrdering() const { return ordering_; }
|
const Ordering& getOrdering() const { return ordering_; }
|
||||||
|
|
||||||
|
/** Access the nonlinear variable index */
|
||||||
|
const VariableIndex& getVariableIndex() const { return variableIndex_; }
|
||||||
|
|
||||||
size_t lastAffectedVariableCount;
|
size_t lastAffectedVariableCount;
|
||||||
size_t lastAffectedFactorCount;
|
size_t lastAffectedFactorCount;
|
||||||
size_t lastAffectedCliqueCount;
|
size_t lastAffectedCliqueCount;
|
||||||
|
|
|
||||||
|
|
@ -34,9 +34,6 @@ void LevenbergMarquardtOptimizer::iterate() {
|
||||||
// Linearize graph
|
// Linearize graph
|
||||||
GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, *params_.ordering);
|
GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, *params_.ordering);
|
||||||
|
|
||||||
// Get elimination method
|
|
||||||
GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
|
|
||||||
|
|
||||||
// Pull out parameters we'll use
|
// Pull out parameters we'll use
|
||||||
const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
|
const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
|
||||||
const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM;
|
const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM;
|
||||||
|
|
@ -69,12 +66,18 @@ void LevenbergMarquardtOptimizer::iterate() {
|
||||||
|
|
||||||
// Optimize
|
// Optimize
|
||||||
VectorValues delta;
|
VectorValues delta;
|
||||||
if(params_.elimination == SuccessiveLinearizationParams::MULTIFRONTAL)
|
if ( params_.isMultifrontal() ) {
|
||||||
delta = GaussianJunctionTree(dampedSystem).optimize(eliminationMethod);
|
delta = GaussianJunctionTree(dampedSystem).optimize(params_.getEliminationFunction());
|
||||||
else if(params_.elimination == SuccessiveLinearizationParams::SEQUENTIAL)
|
}
|
||||||
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(eliminationMethod));
|
else if ( params_.isSequential() ) {
|
||||||
else
|
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction()));
|
||||||
|
}
|
||||||
|
else if ( params_.isCG() ) {
|
||||||
|
throw runtime_error("todo: ");
|
||||||
|
}
|
||||||
|
else {
|
||||||
throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");
|
throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");
|
||||||
|
}
|
||||||
|
|
||||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
|
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
|
||||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
|
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
|
||||||
|
|
|
||||||
|
|
@ -157,11 +157,33 @@ public:
|
||||||
/**
|
/**
|
||||||
* Creates a shared_ptr clone of the factor - needs to be specialized to allow
|
* Creates a shared_ptr clone of the factor - needs to be specialized to allow
|
||||||
* for subclasses
|
* for subclasses
|
||||||
|
*
|
||||||
|
* By default, throws exception if subclass does not implement the function.
|
||||||
*/
|
*/
|
||||||
virtual shared_ptr clone() const =0;
|
virtual shared_ptr clone() const {
|
||||||
|
// TODO: choose better exception to throw here
|
||||||
|
throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
|
||||||
|
return shared_ptr();
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Clones a factor and replaces its keys
|
* Creates a shared_ptr clone of the factor with different keys using
|
||||||
|
* a map from old->new keys
|
||||||
|
*/
|
||||||
|
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const {
|
||||||
|
shared_ptr new_factor = clone();
|
||||||
|
for (size_t i=0; i<new_factor->size(); ++i) {
|
||||||
|
Key& cur_key = new_factor->keys()[i];
|
||||||
|
std::map<Key,Key>::const_iterator mapping = rekey_mapping.find(cur_key);
|
||||||
|
if (mapping != rekey_mapping.end())
|
||||||
|
cur_key = mapping->second;
|
||||||
|
}
|
||||||
|
return new_factor;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Clones a factor and fully replaces its keys
|
||||||
|
* @param new_keys is the full replacement set of keys
|
||||||
*/
|
*/
|
||||||
shared_ptr rekey(const std::vector<Key>& new_keys) const {
|
shared_ptr rekey(const std::vector<Key>& new_keys) const {
|
||||||
assert(new_keys.size() == this->keys().size());
|
assert(new_keys.size() == this->keys().size());
|
||||||
|
|
|
||||||
|
|
@ -27,112 +27,136 @@ using namespace std;
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
double NonlinearFactorGraph::probPrime(const Values& c) const {
|
double NonlinearFactorGraph::probPrime(const Values& c) const {
|
||||||
return exp(-0.5 * error(c));
|
return exp(-0.5 * error(c));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
|
||||||
|
cout << str << "size: " << size() << endl;
|
||||||
|
for (size_t i = 0; i < factors_.size(); i++) {
|
||||||
|
stringstream ss;
|
||||||
|
ss << "factor " << i << ": ";
|
||||||
|
if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
|
double NonlinearFactorGraph::error(const Values& c) const {
|
||||||
cout << str << "size: " << size() << endl;
|
double total_error = 0.;
|
||||||
for (size_t i = 0; i < factors_.size(); i++) {
|
// iterate over all the factors_ to accumulate the log probabilities
|
||||||
stringstream ss;
|
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||||
ss << "factor " << i << ": ";
|
if(factor)
|
||||||
if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
|
total_error += factor->error(c);
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
return total_error;
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
double NonlinearFactorGraph::error(const Values& c) const {
|
std::set<Key> NonlinearFactorGraph::keys() const {
|
||||||
double total_error = 0.;
|
std::set<Key> keys;
|
||||||
// iterate over all the factors_ to accumulate the log probabilities
|
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
if(factor)
|
||||||
if(factor)
|
keys.insert(factor->begin(), factor->end());
|
||||||
total_error += factor->error(c);
|
|
||||||
}
|
|
||||||
return total_error;
|
|
||||||
}
|
}
|
||||||
|
return keys;
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
std::set<Key> NonlinearFactorGraph::keys() const {
|
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
|
||||||
std::set<Key> keys;
|
|
||||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
|
||||||
if(factor)
|
|
||||||
keys.insert(factor->begin(), factor->end());
|
|
||||||
}
|
|
||||||
return keys;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
|
|
||||||
const Values& config) const {
|
|
||||||
|
|
||||||
// Create symbolic graph and initial (iterator) ordering
|
|
||||||
SymbolicFactorGraph::shared_ptr symbolic;
|
|
||||||
Ordering::shared_ptr ordering;
|
|
||||||
boost::tie(symbolic, ordering) = this->symbolic(config);
|
|
||||||
|
|
||||||
// Compute the VariableIndex (column-wise index)
|
|
||||||
VariableIndex variableIndex(*symbolic, ordering->size());
|
|
||||||
if (config.size() != variableIndex.size()) throw std::runtime_error(
|
|
||||||
"orderingCOLAMD: some variables in the graph are not constrained!");
|
|
||||||
|
|
||||||
// Compute a fill-reducing ordering with COLAMD
|
|
||||||
Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD(
|
|
||||||
variableIndex));
|
|
||||||
|
|
||||||
// Permute the Ordering with the COLAMD ordering
|
|
||||||
ordering->permuteWithInverse(*colamdPerm->inverse());
|
|
||||||
|
|
||||||
// Return the Ordering and VariableIndex to be re-used during linearization
|
|
||||||
// and elimination
|
|
||||||
return ordering;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
|
|
||||||
// Generate the symbolic factor graph
|
|
||||||
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
|
|
||||||
symbolicfg->reserve(this->size());
|
|
||||||
|
|
||||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
|
||||||
if(factor)
|
|
||||||
symbolicfg->push_back(factor->symbolic(ordering));
|
|
||||||
else
|
|
||||||
symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
|
|
||||||
}
|
|
||||||
|
|
||||||
return symbolicfg;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
|
|
||||||
const Values& config) const {
|
const Values& config) const {
|
||||||
// Generate an initial key ordering in iterator order
|
|
||||||
Ordering::shared_ptr ordering(config.orderingArbitrary());
|
// Create symbolic graph and initial (iterator) ordering
|
||||||
return make_pair(symbolic(*ordering), ordering);
|
SymbolicFactorGraph::shared_ptr symbolic;
|
||||||
|
Ordering::shared_ptr ordering;
|
||||||
|
boost::tie(symbolic, ordering) = this->symbolic(config);
|
||||||
|
|
||||||
|
// Compute the VariableIndex (column-wise index)
|
||||||
|
VariableIndex variableIndex(*symbolic, ordering->size());
|
||||||
|
if (config.size() != variableIndex.size()) throw std::runtime_error(
|
||||||
|
"orderingCOLAMD: some variables in the graph are not constrained!");
|
||||||
|
|
||||||
|
// Compute a fill-reducing ordering with COLAMD
|
||||||
|
Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD(
|
||||||
|
variableIndex));
|
||||||
|
|
||||||
|
// Permute the Ordering with the COLAMD ordering
|
||||||
|
ordering->permuteWithInverse(*colamdPerm->inverse());
|
||||||
|
|
||||||
|
// Return the Ordering and VariableIndex to be re-used during linearization
|
||||||
|
// and elimination
|
||||||
|
return ordering;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
|
||||||
|
// Generate the symbolic factor graph
|
||||||
|
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
|
||||||
|
symbolicfg->reserve(this->size());
|
||||||
|
|
||||||
|
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||||
|
if(factor)
|
||||||
|
symbolicfg->push_back(factor->symbolic(ordering));
|
||||||
|
else
|
||||||
|
symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
return symbolicfg;
|
||||||
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
|
}
|
||||||
const Values& config, const Ordering& ordering) const {
|
|
||||||
|
|
||||||
// create an empty linear FG
|
/* ************************************************************************* */
|
||||||
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
|
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
|
||||||
linearFG->reserve(this->size());
|
const Values& config) const {
|
||||||
|
// Generate an initial key ordering in iterator order
|
||||||
|
Ordering::shared_ptr ordering(config.orderingArbitrary());
|
||||||
|
return make_pair(symbolic(*ordering), ordering);
|
||||||
|
}
|
||||||
|
|
||||||
// linearize all factors
|
/* ************************************************************************* */
|
||||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
|
||||||
if(factor) {
|
const Values& config, const Ordering& ordering) const {
|
||||||
GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
|
|
||||||
if (lf) linearFG->push_back(lf);
|
|
||||||
} else
|
|
||||||
linearFG->push_back(GaussianFactor::shared_ptr());
|
|
||||||
}
|
|
||||||
|
|
||||||
return linearFG;
|
// create an empty linear FG
|
||||||
|
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
|
||||||
|
linearFG->reserve(this->size());
|
||||||
|
|
||||||
|
// linearize all factors
|
||||||
|
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||||
|
if(factor) {
|
||||||
|
GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
|
||||||
|
if (lf) linearFG->push_back(lf);
|
||||||
|
} else
|
||||||
|
linearFG->push_back(GaussianFactor::shared_ptr());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
return linearFG;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
NonlinearFactorGraph NonlinearFactorGraph::clone() const {
|
||||||
|
NonlinearFactorGraph result;
|
||||||
|
BOOST_FOREACH(const sharedFactor& f, *this) {
|
||||||
|
if (f)
|
||||||
|
result.push_back(f->clone());
|
||||||
|
else
|
||||||
|
result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
|
||||||
|
}
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const {
|
||||||
|
NonlinearFactorGraph result;
|
||||||
|
BOOST_FOREACH(const sharedFactor& f, *this) {
|
||||||
|
if (f)
|
||||||
|
result.push_back(f->rekey(rekey_mapping));
|
||||||
|
else
|
||||||
|
result.push_back(sharedFactor());
|
||||||
|
}
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -87,6 +87,22 @@ namespace gtsam {
|
||||||
boost::shared_ptr<GaussianFactorGraph >
|
boost::shared_ptr<GaussianFactorGraph >
|
||||||
linearize(const Values& config, const Ordering& ordering) const;
|
linearize(const Values& config, const Ordering& ordering) const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Clone() performs a deep-copy of the graph, including all of the factors
|
||||||
|
*/
|
||||||
|
NonlinearFactorGraph clone() const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Rekey() performs a deep-copy of all of the factors, and changes
|
||||||
|
* keys according to a mapping.
|
||||||
|
*
|
||||||
|
* Keys not specified in the mapping will remain unchanged.
|
||||||
|
*
|
||||||
|
* @param rekey_mapping is a map of old->new keys
|
||||||
|
* @result a cloned graph with updated keys
|
||||||
|
*/
|
||||||
|
NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
|
|
|
||||||
|
|
@ -93,7 +93,8 @@ public:
|
||||||
/// behavior of std::map)
|
/// behavior of std::map)
|
||||||
Index& operator[](Key key) {
|
Index& operator[](Key key) {
|
||||||
iterator i=order_.find(key);
|
iterator i=order_.find(key);
|
||||||
if(i == order_.end()) throw std::out_of_range(std::string("Attempting to access a key from an ordering that does not contain that key"));
|
if(i == order_.end()) throw std::out_of_range(
|
||||||
|
std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key));
|
||||||
else return i->second; }
|
else return i->second; }
|
||||||
|
|
||||||
/// Access the index for the requested key, throws std::out_of_range if the
|
/// Access the index for the requested key, throws std::out_of_range if the
|
||||||
|
|
@ -101,7 +102,8 @@ public:
|
||||||
/// behavior of std::map)
|
/// behavior of std::map)
|
||||||
Index operator[](Key key) const {
|
Index operator[](Key key) const {
|
||||||
const_iterator i=order_.find(key);
|
const_iterator i=order_.find(key);
|
||||||
if(i == order_.end()) throw std::out_of_range(std::string("Attempting to access a key from an ordering that does not contain that key"));
|
if(i == order_.end()) throw std::out_of_range(
|
||||||
|
std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key));
|
||||||
else return i->second; }
|
else return i->second; }
|
||||||
|
|
||||||
/** Returns an iterator pointing to the symbol/index pair with the requested,
|
/** Returns an iterator pointing to the symbol/index pair with the requested,
|
||||||
|
|
|
||||||
|
|
@ -19,47 +19,59 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||||
|
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
class SuccessiveLinearizationParams : public NonlinearOptimizerParams {
|
class SuccessiveLinearizationParams : public NonlinearOptimizerParams {
|
||||||
public:
|
public:
|
||||||
/** See SuccessiveLinearizationParams::elimination */
|
/** See SuccessiveLinearizationParams::linearSolverType */
|
||||||
enum Elimination {
|
enum LinearSolverType {
|
||||||
MULTIFRONTAL,
|
MULTIFRONTAL_CHOLESKY,
|
||||||
SEQUENTIAL
|
MULTIFRONTAL_QR,
|
||||||
|
SEQUENTIAL_CHOLESKY,
|
||||||
|
SEQUENTIAL_QR,
|
||||||
|
CHOLMOD, /* Experimental Flag */
|
||||||
|
PCG, /* Experimental Flag */
|
||||||
|
LSPCG /* Experimental Flag */
|
||||||
};
|
};
|
||||||
|
|
||||||
/** See SuccessiveLinearizationParams::factorization */
|
LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
|
||||||
enum Factorization {
|
|
||||||
CHOLESKY,
|
|
||||||
QR,
|
|
||||||
};
|
|
||||||
|
|
||||||
Elimination elimination; ///< The elimination algorithm to use (default: MULTIFRONTAL)
|
|
||||||
Factorization factorization; ///< The numerical factorization (default: Cholesky)
|
|
||||||
boost::optional<Ordering> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty)
|
boost::optional<Ordering> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty)
|
||||||
|
boost::optional<IterativeOptimizationParameters::shared_ptr> iterativeParams; ///< The container for iterativeOptimization parameters.
|
||||||
|
|
||||||
SuccessiveLinearizationParams() :
|
SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {}
|
||||||
elimination(MULTIFRONTAL), factorization(CHOLESKY) {}
|
|
||||||
|
|
||||||
virtual ~SuccessiveLinearizationParams() {}
|
virtual ~SuccessiveLinearizationParams() {}
|
||||||
|
|
||||||
virtual void print(const std::string& str = "") const {
|
virtual void print(const std::string& str = "") const {
|
||||||
NonlinearOptimizerParams::print(str);
|
NonlinearOptimizerParams::print(str);
|
||||||
if(elimination == MULTIFRONTAL)
|
switch ( linearSolverType ) {
|
||||||
std::cout << " elimination method: MULTIFRONTAL\n";
|
case MULTIFRONTAL_CHOLESKY:
|
||||||
else if(elimination == SEQUENTIAL)
|
std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n";
|
||||||
std::cout << " elimination method: SEQUENTIAL\n";
|
break;
|
||||||
else
|
case MULTIFRONTAL_QR:
|
||||||
std::cout << " elimination method: (invalid)\n";
|
std::cout << " linear solver type: MULTIFRONTAL QR\n";
|
||||||
|
break;
|
||||||
if(factorization == CHOLESKY)
|
case SEQUENTIAL_CHOLESKY:
|
||||||
std::cout << " factorization method: CHOLESKY\n";
|
std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n";
|
||||||
else if(factorization == QR)
|
break;
|
||||||
std::cout << " factorization method: QR\n";
|
case SEQUENTIAL_QR:
|
||||||
else
|
std::cout << " linear solver type: SEQUENTIAL QR\n";
|
||||||
std::cout << " factorization method: (invalid)\n";
|
break;
|
||||||
|
case CHOLMOD:
|
||||||
|
std::cout << " linear solver type: CHOLMOD\n";
|
||||||
|
break;
|
||||||
|
case PCG:
|
||||||
|
std::cout << " linear solver type: PCG\n";
|
||||||
|
break;
|
||||||
|
case LSPCG:
|
||||||
|
std::cout << " linear solver type: LSPCG\n";
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
std::cout << " linear solver type: (invalid)\n";
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
if(ordering)
|
if(ordering)
|
||||||
std::cout << " ordering: custom\n";
|
std::cout << " ordering: custom\n";
|
||||||
|
|
@ -69,13 +81,36 @@ public:
|
||||||
std::cout.flush();
|
std::cout.flush();
|
||||||
}
|
}
|
||||||
|
|
||||||
GaussianFactorGraph::Eliminate getEliminationFunction() const {
|
inline bool isMultifrontal() const {
|
||||||
if(factorization == SuccessiveLinearizationParams::CHOLESKY)
|
return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool isSequential() const {
|
||||||
|
return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool isCholmod() const {
|
||||||
|
return (linearSolverType == CHOLMOD);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool isCG() const {
|
||||||
|
return (linearSolverType == PCG || linearSolverType == LSPCG);
|
||||||
|
}
|
||||||
|
|
||||||
|
GaussianFactorGraph::Eliminate getEliminationFunction() {
|
||||||
|
switch (linearSolverType) {
|
||||||
|
case MULTIFRONTAL_CHOLESKY:
|
||||||
|
case MULTIFRONTAL_QR:
|
||||||
return EliminatePreferCholesky;
|
return EliminatePreferCholesky;
|
||||||
else if(factorization == SuccessiveLinearizationParams::QR)
|
|
||||||
|
case SEQUENTIAL_CHOLESKY:
|
||||||
|
case SEQUENTIAL_QR:
|
||||||
return EliminateQR;
|
return EliminateQR;
|
||||||
else
|
|
||||||
|
default:
|
||||||
throw runtime_error("Nonlinear optimization parameter \"factorization\" is invalid");
|
throw runtime_error("Nonlinear optimization parameter \"factorization\" is invalid");
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -7,6 +7,8 @@ set (gtsam_unstable_subdirs
|
||||||
slam
|
slam
|
||||||
)
|
)
|
||||||
|
|
||||||
|
add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
|
||||||
|
|
||||||
# assemble core libaries
|
# assemble core libaries
|
||||||
foreach(subdir ${gtsam_unstable_subdirs})
|
foreach(subdir ${gtsam_unstable_subdirs})
|
||||||
# Build convenience libraries
|
# Build convenience libraries
|
||||||
|
|
@ -86,6 +88,17 @@ if (GTSAM_BUILD_WRAP)
|
||||||
${CMAKE_BINARY_DIR}/wrap/wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR} ${moduleName} ${toolbox_path} "${mexFlags}"
|
${CMAKE_BINARY_DIR}/wrap/wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR} ${moduleName} ${toolbox_path} "${mexFlags}"
|
||||||
DEPENDS wrap)
|
DEPENDS wrap)
|
||||||
|
|
||||||
|
# Build command
|
||||||
|
# Experimental: requires matlab to be on your path
|
||||||
|
if (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
||||||
|
# Actually compile the mex files when building the library
|
||||||
|
set(TOOLBOX_MAKE_FLAGS "-j2")
|
||||||
|
add_custom_target(wrap_gtsam_unstable_build
|
||||||
|
COMMAND make ${TOOLBOX_MAKE_FLAGS}
|
||||||
|
WORKING_DIRECTORY ${toolbox_path}
|
||||||
|
DEPENDS wrap_gtsam_unstable)
|
||||||
|
endif (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
||||||
|
|
||||||
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
||||||
# Primary toolbox files
|
# Primary toolbox files
|
||||||
message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}")
|
message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}")
|
||||||
|
|
|
||||||
|
|
@ -16,4 +16,4 @@ set (base_excluded_tests "")
|
||||||
|
|
||||||
# Add all tests
|
# Add all tests
|
||||||
gtsam_add_subdir_tests(base_unstable "${base_local_libs}" "${base_full_libs}" "${base_excluded_tests}")
|
gtsam_add_subdir_tests(base_unstable "${base_local_libs}" "${base_full_libs}" "${base_excluded_tests}")
|
||||||
|
add_dependencies(check.unstable check.base_unstable)
|
||||||
|
|
|
||||||
|
|
@ -21,7 +21,7 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void AllDiff::print(const std::string& s) const {
|
void AllDiff::print(const std::string& s) const {
|
||||||
std::cout << s << ": AllDiff on ";
|
std::cout << s << "AllDiff on ";
|
||||||
BOOST_FOREACH (Index dkey, keys_)
|
BOOST_FOREACH (Index dkey, keys_)
|
||||||
std::cout << dkey << " ";
|
std::cout << dkey << " ";
|
||||||
std::cout << std::endl;
|
std::cout << std::endl;
|
||||||
|
|
|
||||||
|
|
@ -34,7 +34,7 @@ namespace gtsam {
|
||||||
|
|
||||||
// print
|
// print
|
||||||
virtual void print(const std::string& s = "") const {
|
virtual void print(const std::string& s = "") const {
|
||||||
std::cout << s << ": BinaryAllDiff on " << keys_[0] << " and " << keys_[1]
|
std::cout << s << "BinaryAllDiff on " << keys_[0] << " and " << keys_[1]
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -16,18 +16,19 @@ set (discrete_full_libs
|
||||||
gtsam_unstable-static)
|
gtsam_unstable-static)
|
||||||
|
|
||||||
# Exclude tests that don't work
|
# Exclude tests that don't work
|
||||||
set (discrete_excluded_tests
|
#set (discrete_excluded_tests
|
||||||
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp"
|
#"${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp"
|
||||||
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testCSP.cpp")
|
#)
|
||||||
|
|
||||||
|
|
||||||
# Add all tests
|
# Add all tests
|
||||||
gtsam_add_subdir_tests(discrete_unstable "${discrete_local_libs}" "${discrete_full_libs}" "${discrete_excluded_tests}")
|
gtsam_add_subdir_tests(discrete_unstable "${discrete_local_libs}" "${discrete_full_libs}" "${discrete_excluded_tests}")
|
||||||
|
add_dependencies(check.unstable check.discrete_unstable)
|
||||||
|
|
||||||
# List examples to build - comment out here to exclude from compilation
|
# List examples to build - comment out here to exclude from compilation
|
||||||
set(discrete_unstable_examples
|
set(discrete_unstable_examples
|
||||||
#schedulingExample
|
schedulingExample
|
||||||
#schedulingQuals12
|
schedulingQuals12
|
||||||
)
|
)
|
||||||
|
|
||||||
if (GTSAM_BUILD_EXAMPLES)
|
if (GTSAM_BUILD_EXAMPLES)
|
||||||
|
|
|
||||||
|
|
@ -49,8 +49,9 @@ namespace gtsam {
|
||||||
// if not already a singleton
|
// if not already a singleton
|
||||||
if (!domains[v].isSingleton()) {
|
if (!domains[v].isSingleton()) {
|
||||||
// get the constraint and call its ensureArcConsistency method
|
// get the constraint and call its ensureArcConsistency method
|
||||||
Constraint::shared_ptr factor = (*this)[f];
|
Constraint::shared_ptr constraint = boost::dynamic_pointer_cast<Constraint>((*this)[f]);
|
||||||
changed[v] = factor->ensureArcConsistency(v,domains) || changed[v];
|
if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor");
|
||||||
|
changed[v] = constraint->ensureArcConsistency(v,domains) || changed[v];
|
||||||
}
|
}
|
||||||
} // f
|
} // f
|
||||||
if (changed[v]) anyChange = true;
|
if (changed[v]) anyChange = true;
|
||||||
|
|
@ -84,8 +85,10 @@ namespace gtsam {
|
||||||
// TODO: create a new ordering as we go, to ensure a connected graph
|
// TODO: create a new ordering as we go, to ensure a connected graph
|
||||||
// KeyOrdering ordering;
|
// KeyOrdering ordering;
|
||||||
// vector<Index> dkeys;
|
// vector<Index> dkeys;
|
||||||
BOOST_FOREACH(const Constraint::shared_ptr& factor, factors_) {
|
BOOST_FOREACH(const DiscreteFactor::shared_ptr& f, factors_) {
|
||||||
Constraint::shared_ptr reduced = factor->partiallyApply(domains);
|
Constraint::shared_ptr constraint = boost::dynamic_pointer_cast<Constraint>(f);
|
||||||
|
if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor");
|
||||||
|
Constraint::shared_ptr reduced = constraint->partiallyApply(domains);
|
||||||
if (print) reduced->print();
|
if (print) reduced->print();
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -18,7 +18,7 @@ namespace gtsam {
|
||||||
* A specialization of a DiscreteFactorGraph.
|
* A specialization of a DiscreteFactorGraph.
|
||||||
* It knows about CSP-specific constraints and algorithms
|
* It knows about CSP-specific constraints and algorithms
|
||||||
*/
|
*/
|
||||||
class CSP: public FactorGraph<Constraint> {
|
class CSP: public DiscreteFactorGraph {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
/** A map from keys to values */
|
/** A map from keys to values */
|
||||||
|
|
@ -27,30 +27,10 @@ namespace gtsam {
|
||||||
typedef boost::shared_ptr<Values> sharedValues;
|
typedef boost::shared_ptr<Values> sharedValues;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// Constructor
|
|
||||||
CSP() {
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class SOURCE>
|
// /// Constructor
|
||||||
void add(const DiscreteKey& j, SOURCE table) {
|
// CSP() {
|
||||||
DiscreteKeys keys;
|
// }
|
||||||
keys.push_back(j);
|
|
||||||
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class SOURCE>
|
|
||||||
void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) {
|
|
||||||
DiscreteKeys keys;
|
|
||||||
keys.push_back(j1);
|
|
||||||
keys.push_back(j2);
|
|
||||||
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
|
|
||||||
}
|
|
||||||
|
|
||||||
/** add shared discreteFactor immediately from arguments */
|
|
||||||
template<class SOURCE>
|
|
||||||
void add(const DiscreteKeys& keys, SOURCE table) {
|
|
||||||
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Add a unary constraint, allowing only a single value
|
/// Add a unary constraint, allowing only a single value
|
||||||
void addSingleValue(const DiscreteKey& dkey, size_t value) {
|
void addSingleValue(const DiscreteKey& dkey, size_t value) {
|
||||||
|
|
@ -71,19 +51,28 @@ namespace gtsam {
|
||||||
push_back(factor);
|
push_back(factor);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// /** return product of all factors as a single factor */
|
||||||
|
// DecisionTreeFactor product() const {
|
||||||
|
// DecisionTreeFactor result;
|
||||||
|
// BOOST_FOREACH(const sharedFactor& factor, *this)
|
||||||
|
// if (factor) result = (*factor) * result;
|
||||||
|
// return result;
|
||||||
|
// }
|
||||||
|
|
||||||
/// Find the best total assignment - can be expensive
|
/// Find the best total assignment - can be expensive
|
||||||
sharedValues optimalAssignment() const;
|
sharedValues optimalAssignment() const;
|
||||||
|
|
||||||
/*
|
// /*
|
||||||
* Perform loopy belief propagation
|
// * Perform loopy belief propagation
|
||||||
* True belief propagation would check for each value in domain
|
// * True belief propagation would check for each value in domain
|
||||||
* whether any satisfying separator assignment can be found.
|
// * whether any satisfying separator assignment can be found.
|
||||||
* This corresponds to hyper-arc consistency in CSP speak.
|
// * This corresponds to hyper-arc consistency in CSP speak.
|
||||||
* This can be done by creating a mini-factor graph and search.
|
// * This can be done by creating a mini-factor graph and search.
|
||||||
* For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels deep.
|
// * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels deep.
|
||||||
* It will be very expensive to exclude values that way.
|
// * It will be very expensive to exclude values that way.
|
||||||
*/
|
// */
|
||||||
// void applyBeliefPropagation(size_t nrIterations = 10) const;
|
// void applyBeliefPropagation(size_t nrIterations = 10) const;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Apply arc-consistency ~ Approximate loopy belief propagation
|
* Apply arc-consistency ~ Approximate loopy belief propagation
|
||||||
* We need to give the domains to a constraint, and it returns
|
* We need to give the domains to a constraint, and it returns
|
||||||
|
|
@ -92,7 +81,7 @@ namespace gtsam {
|
||||||
*/
|
*/
|
||||||
void runArcConsistency(size_t cardinality, size_t nrIterations = 10,
|
void runArcConsistency(size_t cardinality, size_t nrIterations = 10,
|
||||||
bool print = false) const;
|
bool print = false) const;
|
||||||
};
|
}; // CSP
|
||||||
|
|
||||||
} // gtsam
|
} // gtsam
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -105,7 +105,6 @@ namespace gtsam {
|
||||||
|
|
||||||
/** Add student-specific constraints to the graph */
|
/** Add student-specific constraints to the graph */
|
||||||
void Scheduler::addStudentSpecificConstraints(size_t i, boost::optional<size_t> slot) {
|
void Scheduler::addStudentSpecificConstraints(size_t i, boost::optional<size_t> slot) {
|
||||||
#ifdef BROKEN
|
|
||||||
bool debug = ISDEBUG("Scheduler::buildGraph");
|
bool debug = ISDEBUG("Scheduler::buildGraph");
|
||||||
|
|
||||||
assert(i<nrStudents());
|
assert(i<nrStudents());
|
||||||
|
|
@ -134,7 +133,7 @@ namespace gtsam {
|
||||||
DiscreteKey dummy(0, nrTimeSlots());
|
DiscreteKey dummy(0, nrTimeSlots());
|
||||||
Potentials::ADT p(dummy & areaKey, available_);
|
Potentials::ADT p(dummy & areaKey, available_);
|
||||||
Potentials::ADT q = p.choose(0, *slot);
|
Potentials::ADT q = p.choose(0, *slot);
|
||||||
Constraint::shared_ptr f(new DecisionTreeFactor(areaKey, q));
|
DiscreteFactor::shared_ptr f(new DecisionTreeFactor(areaKey, q));
|
||||||
CSP::push_back(f);
|
CSP::push_back(f);
|
||||||
} else {
|
} else {
|
||||||
CSP::add(s.key_, areaKey, available_);
|
CSP::add(s.key_, areaKey, available_);
|
||||||
|
|
@ -144,15 +143,11 @@ namespace gtsam {
|
||||||
// add mutex
|
// add mutex
|
||||||
if (debug) cout << "Mutex for faculty" << endl;
|
if (debug) cout << "Mutex for faculty" << endl;
|
||||||
addAllDiff(s.keys_[0] & s.keys_[1] & s.keys_[2]);
|
addAllDiff(s.keys_[0] & s.keys_[1] & s.keys_[2]);
|
||||||
#else
|
|
||||||
throw runtime_error("addStudentSpecificConstraints is broken");
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/** Main routine that builds factor graph */
|
/** Main routine that builds factor graph */
|
||||||
void Scheduler::buildGraph(size_t mutexBound) {
|
void Scheduler::buildGraph(size_t mutexBound) {
|
||||||
#ifdef BROKEN
|
|
||||||
bool debug = ISDEBUG("Scheduler::buildGraph");
|
bool debug = ISDEBUG("Scheduler::buildGraph");
|
||||||
|
|
||||||
if (debug) cout << "Adding student-specific constraints" << endl;
|
if (debug) cout << "Adding student-specific constraints" << endl;
|
||||||
|
|
@ -178,10 +173,6 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#else
|
|
||||||
throw runtime_error("buildGraph is broken");
|
|
||||||
#endif
|
|
||||||
|
|
||||||
} // buildGraph
|
} // buildGraph
|
||||||
|
|
||||||
/** print */
|
/** print */
|
||||||
|
|
|
||||||
|
|
@ -17,8 +17,8 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void SingleValue::print(const string& s) const {
|
void SingleValue::print(const string& s) const {
|
||||||
cout << s << ": SingleValue on " << keys_[0] << " (j=" << keys_[0]
|
cout << s << "SingleValue on " << "j=" << keys_[0]
|
||||||
<< ") with value " << value_ << endl;
|
<< " with value " << value_ << endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -54,17 +54,17 @@ TEST_UNSAFE( CSP, allInOne)
|
||||||
invalid[ID.first] = 0;
|
invalid[ID.first] = 0;
|
||||||
invalid[UT.first] = 0;
|
invalid[UT.first] = 0;
|
||||||
invalid[AZ.first] = 0;
|
invalid[AZ.first] = 0;
|
||||||
EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9); // FIXME: fails due to lack of operator() interface
|
EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9);
|
||||||
|
|
||||||
// Check a valid combination
|
// Check a valid combination
|
||||||
DiscreteFactor::Values valid;
|
DiscreteFactor::Values valid;
|
||||||
valid[ID.first] = 0;
|
valid[ID.first] = 0;
|
||||||
valid[UT.first] = 1;
|
valid[UT.first] = 1;
|
||||||
valid[AZ.first] = 0;
|
valid[AZ.first] = 0;
|
||||||
EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); // FIXME: fails due to lack of operator() interface
|
EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);
|
||||||
|
|
||||||
// Just for fun, create the product and check it
|
// Just for fun, create the product and check it
|
||||||
DecisionTreeFactor product = csp.product(); // FIXME: fails due to lack of product()
|
DecisionTreeFactor product = csp.product();
|
||||||
// product.dot("product");
|
// product.dot("product");
|
||||||
DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0");
|
DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0");
|
||||||
EXPECT(assert_equal(expectedProduct,product));
|
EXPECT(assert_equal(expectedProduct,product));
|
||||||
|
|
@ -74,7 +74,7 @@ TEST_UNSAFE( CSP, allInOne)
|
||||||
CSP::Values expected;
|
CSP::Values expected;
|
||||||
insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1);
|
insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1);
|
||||||
EXPECT(assert_equal(expected,*mpe));
|
EXPECT(assert_equal(expected,*mpe));
|
||||||
EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // FIXME: fails due to lack of operator() interface
|
EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -122,7 +122,7 @@ TEST_UNSAFE( CSP, WesternUS)
|
||||||
(MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2)
|
(MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2)
|
||||||
(ID.first,2)(UT.first,1)(AZ.first,0);
|
(ID.first,2)(UT.first,1)(AZ.first,0);
|
||||||
EXPECT(assert_equal(expected,*mpe));
|
EXPECT(assert_equal(expected,*mpe));
|
||||||
EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // FIXME: fails due to lack of operator() interface
|
EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);
|
||||||
|
|
||||||
// Write out the dual graph for hmetis
|
// Write out the dual graph for hmetis
|
||||||
#ifdef DUAL
|
#ifdef DUAL
|
||||||
|
|
@ -146,7 +146,7 @@ TEST_UNSAFE( CSP, AllDiff)
|
||||||
dkeys += ID,UT,AZ;
|
dkeys += ID,UT,AZ;
|
||||||
csp.addAllDiff(dkeys);
|
csp.addAllDiff(dkeys);
|
||||||
csp.addSingleValue(AZ,2);
|
csp.addSingleValue(AZ,2);
|
||||||
//GTSAM_PRINT(csp);
|
// GTSAM_PRINT(csp);
|
||||||
|
|
||||||
// Check construction and conversion
|
// Check construction and conversion
|
||||||
SingleValue s(AZ,2);
|
SingleValue s(AZ,2);
|
||||||
|
|
@ -167,21 +167,21 @@ TEST_UNSAFE( CSP, AllDiff)
|
||||||
invalid[ID.first] = 0;
|
invalid[ID.first] = 0;
|
||||||
invalid[UT.first] = 1;
|
invalid[UT.first] = 1;
|
||||||
invalid[AZ.first] = 0;
|
invalid[AZ.first] = 0;
|
||||||
EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9); // FIXME: fails due to lack of operator() interface
|
EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9);
|
||||||
|
|
||||||
// Check a valid combination
|
// Check a valid combination
|
||||||
DiscreteFactor::Values valid;
|
DiscreteFactor::Values valid;
|
||||||
valid[ID.first] = 0;
|
valid[ID.first] = 0;
|
||||||
valid[UT.first] = 1;
|
valid[UT.first] = 1;
|
||||||
valid[AZ.first] = 2;
|
valid[AZ.first] = 2;
|
||||||
EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); // FIXME: fails due to lack of operator() interface
|
EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);
|
||||||
|
|
||||||
// Solve
|
// Solve
|
||||||
CSP::sharedValues mpe = csp.optimalAssignment();
|
CSP::sharedValues mpe = csp.optimalAssignment();
|
||||||
CSP::Values expected;
|
CSP::Values expected;
|
||||||
insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2);
|
insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2);
|
||||||
EXPECT(assert_equal(expected,*mpe));
|
EXPECT(assert_equal(expected,*mpe));
|
||||||
EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // FIXME: fails due to lack of operator() interface
|
EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);
|
||||||
|
|
||||||
// Arc-consistency
|
// Arc-consistency
|
||||||
vector<Domain> domains;
|
vector<Domain> domains;
|
||||||
|
|
|
||||||
|
|
@ -14,6 +14,8 @@
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
|
#define PRINT false
|
||||||
|
|
||||||
class Sudoku: public CSP {
|
class Sudoku: public CSP {
|
||||||
|
|
||||||
/// sudoku size
|
/// sudoku size
|
||||||
|
|
@ -119,7 +121,7 @@ TEST_UNSAFE( Sudoku, small)
|
||||||
0,1, 0,0);
|
0,1, 0,0);
|
||||||
|
|
||||||
// Do BP
|
// Do BP
|
||||||
csp.runArcConsistency(4);
|
csp.runArcConsistency(4,10,PRINT);
|
||||||
|
|
||||||
// optimize and check
|
// optimize and check
|
||||||
CSP::sharedValues solution = csp.optimalAssignment();
|
CSP::sharedValues solution = csp.optimalAssignment();
|
||||||
|
|
@ -150,7 +152,7 @@ TEST_UNSAFE( Sudoku, easy)
|
||||||
5,0,0, 0,3,0, 7,0,0);
|
5,0,0, 0,3,0, 7,0,0);
|
||||||
|
|
||||||
// Do BP
|
// Do BP
|
||||||
sudoku.runArcConsistency(4);
|
sudoku.runArcConsistency(4,10,PRINT);
|
||||||
|
|
||||||
// sudoku.printSolution(); // don't do it
|
// sudoku.printSolution(); // don't do it
|
||||||
}
|
}
|
||||||
|
|
@ -172,7 +174,7 @@ TEST_UNSAFE( Sudoku, extreme)
|
||||||
0,0,0, 2,7,5, 9,0,0);
|
0,0,0, 2,7,5, 9,0,0);
|
||||||
|
|
||||||
// Do BP
|
// Do BP
|
||||||
sudoku.runArcConsistency(9,10,false);
|
sudoku.runArcConsistency(9,10,PRINT);
|
||||||
|
|
||||||
#ifdef METIS
|
#ifdef METIS
|
||||||
VariableIndex index(sudoku);
|
VariableIndex index(sudoku);
|
||||||
|
|
@ -201,7 +203,7 @@ TEST_UNSAFE( Sudoku, AJC_3star_Feb8_2012)
|
||||||
0,0,0, 1,0,0, 0,3,7);
|
0,0,0, 1,0,0, 0,3,7);
|
||||||
|
|
||||||
// Do BP
|
// Do BP
|
||||||
sudoku.runArcConsistency(9,10,true);
|
sudoku.runArcConsistency(9,10,PRINT);
|
||||||
|
|
||||||
//sudoku.printSolution(); // don't do it
|
//sudoku.printSolution(); // don't do it
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -23,4 +23,4 @@ set (dynamics_excluded_tests "")
|
||||||
|
|
||||||
# Add all tests
|
# Add all tests
|
||||||
gtsam_add_subdir_tests(dynamics_unstable "${dynamics_local_libs}" "${dynamics_full_libs}" "${dynamics_excluded_tests}")
|
gtsam_add_subdir_tests(dynamics_unstable "${dynamics_local_libs}" "${dynamics_full_libs}" "${dynamics_excluded_tests}")
|
||||||
|
add_dependencies(check.unstable check.dynamics_unstable)
|
||||||
|
|
|
||||||
|
|
@ -23,4 +23,4 @@ set (slam_excluded_tests "")
|
||||||
|
|
||||||
# Add all tests
|
# Add all tests
|
||||||
gtsam_add_subdir_tests(slam_unstable "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}")
|
gtsam_add_subdir_tests(slam_unstable "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}")
|
||||||
|
add_dependencies(check.unstable check.slam_unstable)
|
||||||
|
|
|
||||||
|
|
@ -107,6 +107,43 @@ TEST( Graph, linearize )
|
||||||
CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
|
CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( Graph, clone )
|
||||||
|
{
|
||||||
|
Graph fg = createNonlinearFactorGraph();
|
||||||
|
Graph actClone = fg.clone();
|
||||||
|
EXPECT(assert_equal(fg, actClone));
|
||||||
|
for (size_t i=0; i<fg.size(); ++i)
|
||||||
|
EXPECT(fg[i] != actClone[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( Graph, rekey )
|
||||||
|
{
|
||||||
|
Graph init = createNonlinearFactorGraph();
|
||||||
|
map<Key,Key> rekey_mapping;
|
||||||
|
rekey_mapping.insert(make_pair(kl(1), kl(4)));
|
||||||
|
Graph actRekey = init.rekey(rekey_mapping);
|
||||||
|
|
||||||
|
// ensure deep clone
|
||||||
|
LONGS_EQUAL(init.size(), actRekey.size());
|
||||||
|
for (size_t i=0; i<init.size(); ++i)
|
||||||
|
EXPECT(init[i] != actRekey[i]);
|
||||||
|
|
||||||
|
Graph expRekey;
|
||||||
|
// original measurements
|
||||||
|
expRekey.push_back(init[0]);
|
||||||
|
expRekey.push_back(init[1]);
|
||||||
|
|
||||||
|
// updated measurements
|
||||||
|
Point2 z3(0, -1), z4(-1.5, -1.);
|
||||||
|
SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
|
||||||
|
expRekey.add(simulated2D::Measurement(z3, sigma0_2, kx(1), kl(4)));
|
||||||
|
expRekey.add(simulated2D::Measurement(z4, sigma0_2, kx(2), kl(4)));
|
||||||
|
|
||||||
|
EXPECT(assert_equal(expRekey, actRekey));
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() {
|
int main() {
|
||||||
TestResult tr;
|
TestResult tr;
|
||||||
|
|
|
||||||
|
|
@ -149,9 +149,9 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer )
|
||||||
TEST( NonlinearOptimizer, optimization_method )
|
TEST( NonlinearOptimizer, optimization_method )
|
||||||
{
|
{
|
||||||
LevenbergMarquardtParams paramsQR;
|
LevenbergMarquardtParams paramsQR;
|
||||||
paramsQR.factorization = LevenbergMarquardtParams::QR;
|
paramsQR.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR;
|
||||||
LevenbergMarquardtParams paramsChol;
|
LevenbergMarquardtParams paramsChol;
|
||||||
paramsChol.factorization = LevenbergMarquardtParams::CHOLESKY;
|
paramsChol.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY;
|
||||||
|
|
||||||
example::Graph fg = example::createReallyNonlinearFactorGraph();
|
example::Graph fg = example::createReallyNonlinearFactorGraph();
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -47,6 +47,17 @@ add_custom_target(wrap_gtsam ALL COMMAND
|
||||||
${EXECUTABLE_OUTPUT_PATH}/wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}"
|
${EXECUTABLE_OUTPUT_PATH}/wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}"
|
||||||
DEPENDS wrap)
|
DEPENDS wrap)
|
||||||
|
|
||||||
|
# Build command
|
||||||
|
# Experimental: requires matlab to be on your path
|
||||||
|
if (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
||||||
|
# Actually compile the mex files when building the library
|
||||||
|
set(TOOLBOX_MAKE_FLAGS "-j2")
|
||||||
|
add_custom_target(wrap_gtsam_build
|
||||||
|
COMMAND make ${TOOLBOX_MAKE_FLAGS}
|
||||||
|
WORKING_DIRECTORY ${toolbox_path}
|
||||||
|
DEPENDS wrap_gtsam)
|
||||||
|
endif (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
||||||
|
|
||||||
set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox")
|
set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox")
|
||||||
|
|
||||||
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue