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

652
gtsam/linear/HessianFactor.h
View File

@ -1,326 +1,326 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HessianFactor.h
* @brief Contains the HessianFactor class, a general quadratic factor
* @author Richard Roberts
* @date Dec 8, 2010
*/
#pragma once
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
// Forward declarations for friend unit tests
class ConversionConstructorHessianFactorTest;
class Constructor1HessianFactorTest;
class Constructor1bHessianFactorTest;
class combineHessianFactorTest;
namespace gtsam {
// Forward declarations
class JacobianFactor;
class SharedDiagonal;
class GaussianConditional;
template<class C> class BayesNet;
// Definition of Scatter, which is an intermediate data structure used when
// building a HessianFactor incrementally, to get the keys in the right
// order.
struct SlotEntry {
size_t slot;
size_t dimension;
SlotEntry(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
typedef FastMap<Index, SlotEntry> Scatter;
/**
* @brief A Gaussian factor using the canonical parameters (information form)
*
* HessianFactor implements a general quadratic factor of the form
* \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f]
* that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$.
*
* When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian,
* in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$,
* \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual
* sum-square-error at the mean, when \f$ x = \mu \f$.
*
* Indeed, the negative log-likelihood of a Gaussian is (up to a constant)
* @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$
* with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get
* @f[
* E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu
* @f]
* We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$
* and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$
* to arrive at the canonical form of the Gaussian:
* @f[
* E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu
* @f]
*
* This factor is one of the factors that can be in a GaussianFactorGraph.
* It may be returned from NonlinearFactor::linearize(), but is also
* used internally to store the Hessian during Cholesky elimination.
*
* This can represent a quadratic factor with characteristics that cannot be
* represented using a JacobianFactor (which has the form
* \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$
* and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example,
* a HessianFactor need not be positive semidefinite, it can be indefinite or
* even negative semidefinite.
*
* If a HessianFactor is indefinite or negative semi-definite, then in order
* for solving the linear system to be possible,
* the Hessian of the full system must be positive definite (i.e. when all
* small Hessians are combined, the result must be positive definite). If
* this is not the case, an error will occur during elimination.
*
* This class stores G, g, and f as an augmented matrix HessianFactor::matrix_.
* The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right
* triangle of G, the upper-right-most column of length n of HessianFactor::matrix_
* stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e.
* \code
HessianFactor::matrix_ = [ G11 G12 G13 ... g1
0 G22 G23 ... g2
0 0 G33 ... g3
: : : :
0 0 0 ... f ]
\endcode
Blocks can be accessed as follows:
\code
G11 = info(begin(), begin());
G12 = info(begin(), begin()+1);
G23 = info(begin()+1, begin()+2);
g2 = linearTerm(begin()+1);
f = constantTerm();
.......
\endcode
*/
class HessianFactor : public GaussianFactor {
protected:
typedef Matrix InfoMatrix; ///< The full augmented Hessian
typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
typedef GaussianFactor Base; ///< Typedef to base class
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
BlockInfo info_; ///< The block view of the full information matrix.
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
public:
typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
typedef BlockInfo::Column Column; ///< A column containing the linear term h
typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
/** Copy constructor */
HessianFactor(const HessianFactor& gf);
/** default constructor for I/O */
HessianFactor();
/** Construct a unary factor. G is the quadratic term (Hessian matrix), g
* the linear term (a vector), and f the constant term. The quadratic
* error is:
* 0.5*(f - 2*x'*g + x'*G*x)
*/
HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
/** Construct a unary factor, given a mean and covariance matrix.
* error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
*/
HessianFactor(Index j, const Vector& mu, const Matrix& Sigma);
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
* JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
* HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f]
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
\code
n1*n1 G11 = A1'*M*A1
n1*n2 G12 = A1'*M*A2
n2*n2 G22 = A2'*M*A2
n1*1 g1 = A1'*M*b
n2*1 g2 = A2'*M*b
1*1 f = b'*M*b
\endcode
*/
HessianFactor(Index j1, Index j2,
const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f);
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
HessianFactor(Index j1, Index j2, Index j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f);
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term.
*/
HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f);
/** Construct from Conditional Gaussian */
HessianFactor(const GaussianConditional& cg);
/** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
HessianFactor(const GaussianFactor& factor);
/** Special constructor used in EliminateCholesky which combines the given factors */
HessianFactor(const FactorGraph<GaussianFactor>& factors,
const std::vector<size_t>& dimensions, const Scatter& scatter);
/** Destructor */
virtual ~HessianFactor() {}
/** Aassignment operator */
HessianFactor& operator=(const HessianFactor& rhs);
/** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
shared_ptr(new HessianFactor(*this)));
}
/** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "") const;
/** Compare to another factor for testing (implementing Testable) */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
/** Evaluate the factor error f(x), see above. */
virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
* @param variable An iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
*/
virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); }
/** Return the number of columns and rows of the Hessian matrix */
size_t rows() const { return info_.rows(); }
/** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
* information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation
* above to explain that only the upper-triangular part of the information matrix is stored
* and returned by this function.
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return A view of the requested block, not a copy.
*/
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
/** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
* as described above. See HessianFactor class documentation above to explain that only the
* upper-triangular part of the information matrix is stored and returned by this function.
*/
constBlock info() const { return info_.full(); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/
double constantTerm() const;
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return The linear term \f$ g \f$ */
constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
constColumn linearTerm() 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.
*
* For HessianFactor, this is the same as info() except that this function
* returns a complete symmetric matrix whereas info() returns a matrix where
* only the upper triangle is valid, but should be interpreted as symmetric.
* This is because info() returns only a reference to the internal
* representation of the augmented information matrix, which stores only the
* upper triangle.
*/
virtual Matrix computeInformation() const;
// Friend unit test classes
friend class ::ConversionConstructorHessianFactorTest;
friend class ::Constructor1HessianFactorTest;
friend class ::Constructor1bHessianFactorTest;
friend class ::combineHessianFactorTest;
// Friend JacobianFactor for conversion
friend class JacobianFactor;
// used in eliminateCholesky:
/**
* Do Cholesky. Note that after this, the lower triangle still contains
* some untouched non-zeros that should be zero. We zero them while
* extracting submatrices in splitEliminatedFactor. Frank says :-(
*/
void partialCholesky(size_t nrFrontals);
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
/** assert invariants */
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
ar & BOOST_SERIALIZATION_NVP(info_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
};
}
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HessianFactor.h
* @brief Contains the HessianFactor class, a general quadratic factor
* @author Richard Roberts
* @date Dec 8, 2010
*/
#pragma once
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
// Forward declarations for friend unit tests
class ConversionConstructorHessianFactorTest;
class Constructor1HessianFactorTest;
class Constructor1bHessianFactorTest;
class combineHessianFactorTest;
namespace gtsam {
// Forward declarations
class JacobianFactor;
class SharedDiagonal;
class GaussianConditional;
template<class C> class BayesNet;
// Definition of Scatter, which is an intermediate data structure used when
// building a HessianFactor incrementally, to get the keys in the right
// order.
struct SlotEntry {
size_t slot;
size_t dimension;
SlotEntry(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
typedef FastMap<Index, SlotEntry> Scatter;
/**
* @brief A Gaussian factor using the canonical parameters (information form)
*
* HessianFactor implements a general quadratic factor of the form
* \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f]
* that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$.
*
* When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian,
* in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$,
* \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual
* sum-square-error at the mean, when \f$ x = \mu \f$.
*
* Indeed, the negative log-likelihood of a Gaussian is (up to a constant)
* @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$
* with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get
* @f[
* E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu
* @f]
* We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$
* and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$
* to arrive at the canonical form of the Gaussian:
* @f[
* E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu
* @f]
*
* This factor is one of the factors that can be in a GaussianFactorGraph.
* It may be returned from NonlinearFactor::linearize(), but is also
* used internally to store the Hessian during Cholesky elimination.
*
* This can represent a quadratic factor with characteristics that cannot be
* represented using a JacobianFactor (which has the form
* \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$
* and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example,
* a HessianFactor need not be positive semidefinite, it can be indefinite or
* even negative semidefinite.
*
* If a HessianFactor is indefinite or negative semi-definite, then in order
* for solving the linear system to be possible,
* the Hessian of the full system must be positive definite (i.e. when all
* small Hessians are combined, the result must be positive definite). If
* this is not the case, an error will occur during elimination.
*
* This class stores G, g, and f as an augmented matrix HessianFactor::matrix_.
* The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right
* triangle of G, the upper-right-most column of length n of HessianFactor::matrix_
* stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e.
* \code
HessianFactor::matrix_ = [ G11 G12 G13 ... g1
0 G22 G23 ... g2
0 0 G33 ... g3
: : : :
0 0 0 ... f ]
\endcode
Blocks can be accessed as follows:
\code
G11 = info(begin(), begin());
G12 = info(begin(), begin()+1);
G23 = info(begin()+1, begin()+2);
g2 = linearTerm(begin()+1);
f = constantTerm();
.......
\endcode
*/
class HessianFactor : public GaussianFactor {
protected:
typedef Matrix InfoMatrix; ///< The full augmented Hessian
typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
typedef GaussianFactor Base; ///< Typedef to base class
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
BlockInfo info_; ///< The block view of the full information matrix.
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
public:
typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
typedef BlockInfo::Column Column; ///< A column containing the linear term h
typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
/** Copy constructor */
HessianFactor(const HessianFactor& gf);
/** default constructor for I/O */
HessianFactor();
/** Construct a unary factor. G is the quadratic term (Hessian matrix), g
* the linear term (a vector), and f the constant term. The quadratic
* error is:
* 0.5*(f - 2*x'*g + x'*G*x)
*/
HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
/** Construct a unary factor, given a mean and covariance matrix.
* error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
*/
HessianFactor(Index j, const Vector& mu, const Matrix& Sigma);
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
* JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
* HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f]
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
\code
n1*n1 G11 = A1'*M*A1
n1*n2 G12 = A1'*M*A2
n2*n2 G22 = A2'*M*A2
n1*1 g1 = A1'*M*b
n2*1 g2 = A2'*M*b
1*1 f = b'*M*b
\endcode
*/
HessianFactor(Index j1, Index j2,
const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f);
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
HessianFactor(Index j1, Index j2, Index j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f);
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term.
*/
HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f);
/** Construct from Conditional Gaussian */
HessianFactor(const GaussianConditional& cg);
/** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
HessianFactor(const GaussianFactor& factor);
/** Special constructor used in EliminateCholesky which combines the given factors */
HessianFactor(const FactorGraph<GaussianFactor>& factors,
const std::vector<size_t>& dimensions, const Scatter& scatter);
/** Destructor */
virtual ~HessianFactor() {}
/** Aassignment operator */
HessianFactor& operator=(const HessianFactor& rhs);
/** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
shared_ptr(new HessianFactor(*this)));
}
/** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "") const;
/** Compare to another factor for testing (implementing Testable) */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
/** Evaluate the factor error f(x), see above. */
virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
* @param variable An iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
*/
virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); }
/** Return the number of columns and rows of the Hessian matrix */
size_t rows() const { return info_.rows(); }
/** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
* information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation
* above to explain that only the upper-triangular part of the information matrix is stored
* and returned by this function.
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return A view of the requested block, not a copy.
*/
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
/** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
* as described above. See HessianFactor class documentation above to explain that only the
* upper-triangular part of the information matrix is stored and returned by this function.
*/
constBlock info() const { return info_.full(); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/
double constantTerm() const;
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return The linear term \f$ g \f$ */
constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
constColumn linearTerm() 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.
*
* For HessianFactor, this is the same as info() except that this function
* returns a complete symmetric matrix whereas info() returns a matrix where
* only the upper triangle is valid, but should be interpreted as symmetric.
* This is because info() returns only a reference to the internal
* representation of the augmented information matrix, which stores only the
* upper triangle.
*/
virtual Matrix computeInformation() const;
// Friend unit test classes
friend class ::ConversionConstructorHessianFactorTest;
friend class ::Constructor1HessianFactorTest;
friend class ::Constructor1bHessianFactorTest;
friend class ::combineHessianFactorTest;
// Friend JacobianFactor for conversion
friend class JacobianFactor;
// used in eliminateCholesky:
/**
* Do Cholesky. Note that after this, the lower triangle still contains
* some untouched non-zeros that should be zero. We zero them while
* extracting submatrices in splitEliminatedFactor. Frank says :-(
*/
void partialCholesky(size_t nrFrontals);
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
/** assert invariants */
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
ar & BOOST_SERIALIZATION_NVP(info_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
};
}

10
gtsam/linear/JacobianFactor.h
View File

@ -154,6 +154,16 @@ namespace gtsam {
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
* additional column holding the information vector, and an additional row
* holding the transpose of the information vector. The lower-right entry
* contains the constant error term (when \f$ \delta x = 0 \f$). The
* augmented information matrix is described in more detail in HessianFactor,
* which in fact stores an augmented information matrix.
*/
virtual Matrix computeInformation() const;
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an

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,13 +35,18 @@ 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

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

208
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -27,112 +27,136 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
double NonlinearFactorGraph::probPrime(const Values& c) const {
return exp(-0.5 * error(c));
/* ************************************************************************* */
double NonlinearFactorGraph::probPrime(const Values& c) const {
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 {
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);
}
/* ************************************************************************* */
double NonlinearFactorGraph::error(const Values& c) const {
double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
total_error += factor->error(c);
}
return total_error;
}
/* ************************************************************************* */
double NonlinearFactorGraph::error(const Values& c) const {
double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
total_error += factor->error(c);
}
return total_error;
/* ************************************************************************* */
std::set<Key> NonlinearFactorGraph::keys() const {
std::set<Key> keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
keys.insert(factor->begin(), factor->end());
}
return keys;
}
/* ************************************************************************* */
std::set<Key> NonlinearFactorGraph::keys() const {
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(
/* ************************************************************************* */
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
const Values& config) const {
// Generate an initial key ordering in iterator order
Ordering::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(*ordering), ordering);
// 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());
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
const Values& config, const Ordering& ordering) const {
return symbolicfg;
}
// create an empty linear FG
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
linearFG->reserve(this->size());
/* ************************************************************************* */
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
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_) {
if(factor) {
GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
if (lf) linearFG->push_back(lf);
} else
linearFG->push_back(GaussianFactor::shared_ptr());
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
const Values& config, const Ordering& ordering) const {
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

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

59
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.
*/
// void applyBeliefPropagation(size_t nrIterations = 10) 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.
// */
// 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;
}
/* ************************************************************************* */

18
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
@ -146,7 +146,7 @@ TEST_UNSAFE( CSP, AllDiff)
dkeys += ID,UT,AZ;
csp.addAllDiff(dkeys);
csp.addSingleValue(AZ,2);
//GTSAM_PRINT(csp);
// GTSAM_PRINT(csp);
// Check construction and conversion
SingleValue s(AZ,2);
@ -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)