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

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

Conflicts:
	gtsam/linear/HessianFactor.h
	gtsam/nonlinear/Marginals.cpp
release/4.3a0
Richard Roberts 2012-05-28 12:22:36 +00:00
parent 2a98ba5857 3c694e25f7
commit 1ca9e7049e
35 changed files with 893 additions and 676 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

280
.cproject
View File

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

3
CMakeLists.txt
View File

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

4
CppUnitLite/SimpleString.cpp
View File

@ -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_);

2
examples/PlanarSLAMSelfContained_advanced.cpp
View File

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

@ -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);
}; };

2
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -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:");
} }

2
gtsam/discrete/DecisionTreeFactor.h
View File

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

2
gtsam/discrete/DiscreteFactor.h
View File

@ -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);
} }

10
gtsam/linear/JacobianFactor.h
View File

@ -165,6 +165,16 @@ namespace gtsam {
*/ */
virtual Matrix computeInformation() const; virtual Matrix computeInformation() const;
/** 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;
/** Check if the factor contains no information, i.e. zero rows. This does /** Check if the factor contains no information, i.e. zero rows. This does
* not necessarily mean that the factor involves no variables (to check for * not necessarily mean that the factor involves no variables (to check for
* involving no variables use keys().empty()). * involving no variables use keys().empty()).

20
gtsam/nonlinear/DoglegOptimizer.cpp
View File

@ -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");
} }

17
gtsam/nonlinear/GaussNewtonOptimizer.cpp
View File

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

3
gtsam/nonlinear/ISAM2.h
View File

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

19
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -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");

26
gtsam/nonlinear/NonlinearFactor.h
View File

@ -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());

208
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

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

16
gtsam/nonlinear/NonlinearFactorGraph.h
View File

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

6
gtsam/nonlinear/Ordering.h
View File

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

97
gtsam/nonlinear/SuccessiveLinearizationOptimizer.h
View File

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

13
gtsam_unstable/CMakeLists.txt
View File

@ -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}")

2
gtsam_unstable/base/CMakeLists.txt
View File

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

2
gtsam_unstable/discrete/AllDiff.cpp
View File

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

2
gtsam_unstable/discrete/BinaryAllDiff.h
View File

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

11
gtsam_unstable/discrete/CMakeLists.txt
View File

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

11
gtsam_unstable/discrete/CSP.cpp
View File

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

59
gtsam_unstable/discrete/CSP.h
View File

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

11
gtsam_unstable/discrete/Scheduler.cpp
View File

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

4
gtsam_unstable/discrete/SingleValue.cpp
View File

@ -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;
} }
/* ************************************************************************* */ /* ************************************************************************* */

18
gtsam_unstable/discrete/tests/testCSP.cpp
View File

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

10
gtsam_unstable/discrete/tests/testSudoku.cpp
View File

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

2
gtsam_unstable/dynamics/CMakeLists.txt
View File

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

2
gtsam_unstable/slam/CMakeLists.txt
View File

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

37
tests/testNonlinearFactorGraph.cpp
View File

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

4
tests/testNonlinearOptimizer.cpp
View File

@ -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();

11
wrap/CMakeLists.txt
View File

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