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>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -705,42 +737,34 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testKey.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPlanarSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testPlanarSLAM.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2SLAM.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>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -769,6 +793,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -801,10 +865,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>check</buildTarget>
<buildTarget>testDiscreteFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -985,6 +1049,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1600,82 +1672,74 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/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">
<target name="testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>linear.testNoiseModel.run</buildTarget>
<buildTarget>testVectorValues.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianFactor.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">
<target name="testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<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>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1768,22 +1832,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="LocalizationExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>LocalizationExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="LocalizationExample2.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>LocalizationExample2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2147,18 +2195,18 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_unstable</buildTarget>
<buildTarget>check.discrete_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<buildTarget>check.base_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -2179,26 +2227,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>check.base_unstable</buildTarget>
<buildTarget>check.unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<buildTarget>wrap.testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="wrap.testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWrap.run</buildTarget>
<buildTarget>wrap.testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<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_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
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source 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)
{
delete buffer_;
delete [] buffer_;
buffer_ = new char [other.size() + 1];
strcpy(buffer_, other.buffer_);
return *this;
@ -50,7 +50,7 @@ SimpleString SimpleString::operator= (const SimpleString& other)
SimpleString SimpleString::operator+ (const SimpleString& other)
{
SimpleString ret;
delete ret.buffer_;
delete [] ret.buffer_;
ret.buffer_ = new char [size() + other.size() - 1];
strcpy(ret.buffer_, 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
LevenbergMarquardtParams lmParams;
lmParams.elimination = LevenbergMarquardtParams::SEQUENTIAL;
lmParams.linearSolverType = LevenbergMarquardtParams::SEQUENTIAL_CHOLESKY;
Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
resultSequential.print("final result (solved with a sequential solver)");

1
gtsam.h
View File

@ -489,7 +489,6 @@ class Values {
Values();
void print(string s) const;
void insertPose(int key, const gtsam::Pose2& pose);
gtsam::Symbol poseKey(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 {
cout << s << ":\n";
cout << s;
IndexFactor::print("IndexFactor:");
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;
// print
void print(const std::string& s = "DecisionTreeFactor: ") const;
void print(const std::string& s = "DecisionTreeFactor:\n") const;
/// @}
/// @name Standard Interface

2
gtsam/discrete/DiscreteFactor.h
View File

@ -82,7 +82,7 @@ namespace gtsam {
/// @{
// print
virtual void print(const std::string& s = "DiscreteFactor") const {
virtual void print(const std::string& s = "DiscreteFactor\n") const {
IndexFactor::print(s);
}

10
gtsam/linear/JacobianFactor.h
View File

@ -165,6 +165,16 @@ namespace gtsam {
*/
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
* not necessarily mean that the factor involves no variables (to check for
* 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;
GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, ordering);
// Get elimination method
GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
// Pull out parameters we'll use
const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT);
// Do Dogleg iteration with either Multifrontal or Sequential elimination
DoglegOptimizerImpl::IterationResult result;
if(params_.elimination == DoglegParams::MULTIFRONTAL) {
if ( params_.isMultifrontal() ) {
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);
} else if(params_.elimination == DoglegParams::SEQUENTIAL) {
GaussianBayesNet::shared_ptr bn = EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod);
}
else if ( params_.isSequential() ) {
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);
} else {
}
else if ( params_.isCG() ) {
throw runtime_error("todo: ");
}
else {
throw runtime_error("Optimization parameter is invalid: DoglegParams::elimination");
}

17
gtsam/nonlinear/GaussNewtonOptimizer.cpp
View File

@ -35,14 +35,19 @@ void GaussNewtonOptimizer::iterate() {
// Optimize
VectorValues delta;
{
GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
if(params_.elimination == GaussNewtonParams::MULTIFRONTAL)
delta = GaussianJunctionTree(*linear).optimize(eliminationMethod);
else if(params_.elimination == GaussNewtonParams::SEQUENTIAL)
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod));
else
if ( params_.isMultifrontal() ) {
delta = GaussianJunctionTree(*linear).optimize(params_.getEliminationFunction());
}
else if ( params_.isSequential() ) {
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(params_.getEliminationFunction()));
}
else if ( params_.isCG() ) {
throw runtime_error("todo: ");
}
else {
throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination");
}
}
// Maybe show output
if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta");

3
gtsam/nonlinear/ISAM2.h
View File

@ -490,6 +490,9 @@ public:
/** Access the current ordering */
const Ordering& getOrdering() const { return ordering_; }
/** Access the nonlinear variable index */
const VariableIndex& getVariableIndex() const { return variableIndex_; }
size_t lastAffectedVariableCount;
size_t lastAffectedFactorCount;
size_t lastAffectedCliqueCount;

19
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -34,9 +34,6 @@ void LevenbergMarquardtOptimizer::iterate() {
// Linearize graph
GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, *params_.ordering);
// Get elimination method
GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
// Pull out parameters we'll use
const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM;
@ -69,12 +66,18 @@ void LevenbergMarquardtOptimizer::iterate() {
// Optimize
VectorValues delta;
if(params_.elimination == SuccessiveLinearizationParams::MULTIFRONTAL)
delta = GaussianJunctionTree(dampedSystem).optimize(eliminationMethod);
else if(params_.elimination == SuccessiveLinearizationParams::SEQUENTIAL)
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(eliminationMethod));
else
if ( params_.isMultifrontal() ) {
delta = GaussianJunctionTree(dampedSystem).optimize(params_.getEliminationFunction());
}
else if ( params_.isSequential() ) {
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");
}
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
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
* 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 {
assert(new_keys.size() == this->keys().size());

24
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -133,6 +133,30 @@ namespace gtsam {
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

16
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -87,6 +87,22 @@ namespace gtsam {
boost::shared_ptr<GaussianFactorGraph >
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:
/** Serialization function */

6
gtsam/nonlinear/Ordering.h
View File

@ -93,7 +93,8 @@ public:
/// behavior of std::map)
Index& operator[](Key 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; }
/// Access the index for the requested key, throws std::out_of_range if the
@ -101,7 +102,8 @@ public:
/// behavior of std::map)
Index operator[](Key key) const {
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; }
/** 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
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/IterativeOptimizationParameters.h>
namespace gtsam {
class SuccessiveLinearizationParams : public NonlinearOptimizerParams {
public:
/** See SuccessiveLinearizationParams::elimination */
enum Elimination {
MULTIFRONTAL,
SEQUENTIAL
/** See SuccessiveLinearizationParams::linearSolverType */
enum LinearSolverType {
MULTIFRONTAL_CHOLESKY,
MULTIFRONTAL_QR,
SEQUENTIAL_CHOLESKY,
SEQUENTIAL_QR,
CHOLMOD, /* Experimental Flag */
PCG, /* Experimental Flag */
LSPCG /* Experimental Flag */
};
/** See SuccessiveLinearizationParams::factorization */
enum Factorization {
CHOLESKY,
QR,
};
Elimination elimination; ///< The elimination algorithm to use (default: MULTIFRONTAL)
Factorization factorization; ///< The numerical factorization (default: Cholesky)
LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
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() :
elimination(MULTIFRONTAL), factorization(CHOLESKY) {}
SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {}
virtual ~SuccessiveLinearizationParams() {}
virtual void print(const std::string& str = "") const {
NonlinearOptimizerParams::print(str);
if(elimination == MULTIFRONTAL)
std::cout << " elimination method: MULTIFRONTAL\n";
else if(elimination == SEQUENTIAL)
std::cout << " elimination method: SEQUENTIAL\n";
else
std::cout << " elimination method: (invalid)\n";
if(factorization == CHOLESKY)
std::cout << " factorization method: CHOLESKY\n";
else if(factorization == QR)
std::cout << " factorization method: QR\n";
else
std::cout << " factorization method: (invalid)\n";
switch ( linearSolverType ) {
case MULTIFRONTAL_CHOLESKY:
std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n";
break;
case MULTIFRONTAL_QR:
std::cout << " linear solver type: MULTIFRONTAL QR\n";
break;
case SEQUENTIAL_CHOLESKY:
std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n";
break;
case SEQUENTIAL_QR:
std::cout << " linear solver type: SEQUENTIAL QR\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)
std::cout << " ordering: custom\n";
@ -69,13 +81,36 @@ public:
std::cout.flush();
}
GaussianFactorGraph::Eliminate getEliminationFunction() const {
if(factorization == SuccessiveLinearizationParams::CHOLESKY)
inline bool isMultifrontal() const {
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;
else if(factorization == SuccessiveLinearizationParams::QR)
case SEQUENTIAL_CHOLESKY:
case SEQUENTIAL_QR:
return EliminateQR;
else
default:
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
)
add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
# assemble core libaries
foreach(subdir ${gtsam_unstable_subdirs})
# 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}"
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)
# Primary toolbox files
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
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 {
std::cout << s << ": AllDiff on ";
std::cout << s << "AllDiff on ";
BOOST_FOREACH (Index dkey, keys_)
std::cout << dkey << " ";
std::cout << std::endl;

2
gtsam_unstable/discrete/BinaryAllDiff.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
// print
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;
}

11
gtsam_unstable/discrete/CMakeLists.txt
View File

@ -16,18 +16,19 @@ set (discrete_full_libs
gtsam_unstable-static)
# Exclude tests that don't work
set (discrete_excluded_tests
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testCSP.cpp")
#set (discrete_excluded_tests
#"${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp"
#)
# Add all 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
set(discrete_unstable_examples
#schedulingExample
#schedulingQuals12
schedulingExample
schedulingQuals12
)
if (GTSAM_BUILD_EXAMPLES)

11
gtsam_unstable/discrete/CSP.cpp
View File

@ -49,8 +49,9 @@ namespace gtsam {
// if not already a singleton
if (!domains[v].isSingleton()) {
// get the constraint and call its ensureArcConsistency method
Constraint::shared_ptr factor = (*this)[f];
changed[v] = factor->ensureArcConsistency(v,domains) || changed[v];
Constraint::shared_ptr constraint = boost::dynamic_pointer_cast<Constraint>((*this)[f]);
if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor");
changed[v] = constraint->ensureArcConsistency(v,domains) || changed[v];
}
} // f
if (changed[v]) anyChange = true;
@ -84,8 +85,10 @@ namespace gtsam {
// TODO: create a new ordering as we go, to ensure a connected graph
// KeyOrdering ordering;
// vector<Index> dkeys;
BOOST_FOREACH(const Constraint::shared_ptr& factor, factors_) {
Constraint::shared_ptr reduced = factor->partiallyApply(domains);
BOOST_FOREACH(const DiscreteFactor::shared_ptr& f, factors_) {
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();
}
#endif

57
gtsam_unstable/discrete/CSP.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
* A specialization of a DiscreteFactorGraph.
* It knows about CSP-specific constraints and algorithms
*/
class CSP: public FactorGraph<Constraint> {
class CSP: public DiscreteFactorGraph {
public:
/** A map from keys to values */
@ -27,30 +27,10 @@ namespace gtsam {
typedef boost::shared_ptr<Values> sharedValues;
public:
/// Constructor
CSP() {
}
template<class SOURCE>
void add(const DiscreteKey& j, SOURCE table) {
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));
}
// /// Constructor
// CSP() {
// }
/// Add a unary constraint, allowing only a single value
void addSingleValue(const DiscreteKey& dkey, size_t value) {
@ -71,19 +51,28 @@ namespace gtsam {
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
sharedValues optimalAssignment() const;
/*
* Perform loopy belief propagation
* True belief propagation would check for each value in domain
* whether any satisfying separator assignment can be found.
* This corresponds to hyper-arc consistency in CSP speak.
* 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.
* It will be very expensive to exclude values that way.
*/
// /*
// * Perform loopy belief propagation
// * True belief propagation would check for each value in domain
// * whether any satisfying separator assignment can be found.
// * This corresponds to hyper-arc consistency in CSP speak.
// * 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.
// * It will be very expensive to exclude values that way.
// */
// void applyBeliefPropagation(size_t nrIterations = 10) const;
/*
* Apply arc-consistency ~ Approximate loopy belief propagation
* 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,
bool print = false) const;
};
}; // CSP
} // gtsam

11
gtsam_unstable/discrete/Scheduler.cpp
View File

@ -105,7 +105,6 @@ namespace gtsam {
/** Add student-specific constraints to the graph */
void Scheduler::addStudentSpecificConstraints(size_t i, boost::optional<size_t> slot) {
#ifdef BROKEN
bool debug = ISDEBUG("Scheduler::buildGraph");
assert(i<nrStudents());
@ -134,7 +133,7 @@ namespace gtsam {
DiscreteKey dummy(0, nrTimeSlots());
Potentials::ADT p(dummy & areaKey, available_);
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);
} else {
CSP::add(s.key_, areaKey, available_);
@ -144,15 +143,11 @@ namespace gtsam {
// add mutex
if (debug) cout << "Mutex for faculty" << endl;
addAllDiff(s.keys_[0] & s.keys_[1] & s.keys_[2]);
#else
throw runtime_error("addStudentSpecificConstraints is broken");
#endif
}
/** Main routine that builds factor graph */
void Scheduler::buildGraph(size_t mutexBound) {
#ifdef BROKEN
bool debug = ISDEBUG("Scheduler::buildGraph");
if (debug) cout << "Adding student-specific constraints" << endl;
@ -178,10 +173,6 @@ namespace gtsam {
}
}
}
#else
throw runtime_error("buildGraph is broken");
#endif
} // buildGraph
/** print */

4
gtsam_unstable/discrete/SingleValue.cpp
View File

@ -17,8 +17,8 @@ namespace gtsam {
/* ************************************************************************* */
void SingleValue::print(const string& s) const {
cout << s << ": SingleValue on " << keys_[0] << " (j=" << keys_[0]
<< ") with value " << value_ << endl;
cout << s << "SingleValue on " << "j=" << keys_[0]
<< " with value " << value_ << endl;
}
/* ************************************************************************* */

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

@ -54,17 +54,17 @@ TEST_UNSAFE( CSP, allInOne)
invalid[ID.first] = 0;
invalid[UT.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
DiscreteFactor::Values valid;
valid[ID.first] = 0;
valid[UT.first] = 1;
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
DecisionTreeFactor product = csp.product(); // FIXME: fails due to lack of product()
DecisionTreeFactor product = csp.product();
// product.dot("product");
DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0");
EXPECT(assert_equal(expectedProduct,product));
@ -74,7 +74,7 @@ TEST_UNSAFE( CSP, allInOne)
CSP::Values expected;
insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1);
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)
(ID.first,2)(UT.first,1)(AZ.first,0);
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
#ifdef DUAL
@ -167,21 +167,21 @@ TEST_UNSAFE( CSP, AllDiff)
invalid[ID.first] = 0;
invalid[UT.first] = 1;
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
DiscreteFactor::Values valid;
valid[ID.first] = 0;
valid[UT.first] = 1;
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
CSP::sharedValues mpe = csp.optimalAssignment();
CSP::Values expected;
insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2);
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
vector<Domain> domains;

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

@ -14,6 +14,8 @@
using namespace std;
using namespace gtsam;
#define PRINT false
class Sudoku: public CSP {
/// sudoku size
@ -119,7 +121,7 @@ TEST_UNSAFE( Sudoku, small)
0,1, 0,0);
// Do BP
csp.runArcConsistency(4);
csp.runArcConsistency(4,10,PRINT);
// optimize and check
CSP::sharedValues solution = csp.optimalAssignment();
@ -150,7 +152,7 @@ TEST_UNSAFE( Sudoku, easy)
5,0,0, 0,3,0, 7,0,0);
// Do BP
sudoku.runArcConsistency(4);
sudoku.runArcConsistency(4,10,PRINT);
// sudoku.printSolution(); // don't do it
}
@ -172,7 +174,7 @@ TEST_UNSAFE( Sudoku, extreme)
0,0,0, 2,7,5, 9,0,0);
// Do BP
sudoku.runArcConsistency(9,10,false);
sudoku.runArcConsistency(9,10,PRINT);
#ifdef METIS
VariableIndex index(sudoku);
@ -201,7 +203,7 @@ TEST_UNSAFE( Sudoku, AJC_3star_Feb8_2012)
0,0,0, 1,0,0, 0,3,7);
// Do BP
sudoku.runArcConsistency(9,10,true);
sudoku.runArcConsistency(9,10,PRINT);
//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
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
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
}
/* ************************************************************************* */
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() {
TestResult tr;

4
tests/testNonlinearOptimizer.cpp
View File

@ -149,9 +149,9 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer )
TEST( NonlinearOptimizer, optimization_method )
{
LevenbergMarquardtParams paramsQR;
paramsQR.factorization = LevenbergMarquardtParams::QR;
paramsQR.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR;
LevenbergMarquardtParams paramsChol;
paramsChol.factorization = LevenbergMarquardtParams::CHOLESKY;
paramsChol.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY;
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}"
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")
if (GTSAM_INSTALL_MATLAB_TOOLBOX)