diff --git a/.cproject b/.cproject index 9cd83790e..0acb12e7e 100644 --- a/.cproject +++ b/.cproject @@ -593,6 +593,38 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -705,42 +737,34 @@ true true - + make - -j5 - testKey.run + -j2 + tests/testGeneralSFMFactor.run true true true - + make - -j5 - testGeneralSFMFactor.run + -j2 + tests/testPlanarSLAM.run true true true - + make - -j5 - testPlanarSLAM.run + -j2 + tests/testPose2SLAM.run true true true - + make - -j5 - testPose2SLAM.run - true - true - true - - - make - -j5 - testPose3SLAM.run + -j2 + tests/testPose3SLAM.run true true true @@ -769,6 +793,46 @@ true true + + make + -j5 + schedulingExample.run + true + true + true + + + make + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + schedulingQuals12.run + true + true + true + + + make + -j5 + testSudoku.run + true + true + true + make -j2 @@ -801,10 +865,10 @@ true true - + make -j5 - check + testDiscreteFactor.run true true true @@ -985,6 +1049,14 @@ true true + + make + -j5 + testNonlinearFactor.run + true + true + true + make -j2 @@ -1600,82 +1672,74 @@ true true - - make - -j2 - tests/testVectorValues.run - true - true - true - - + make -j5 - linear.testNoiseModel.run + testVectorValues.run true true true - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testHessianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/testGaussianFactorGraph.run - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testKalmanFilter.run - true - true - true - - - make - -j2 - tests/testGaussianDensity.run - true - true - true - - + make -j5 - linear.testSerializationLinear.run + testNoiseModel.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianFactorGraph.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testSerializationLinear.run true true true @@ -1768,22 +1832,6 @@ true true - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - LocalizationExample2.run - true - true - true - make -j2 @@ -2147,18 +2195,18 @@ true true - + make -j5 - wrap_gtsam_unstable + check.discrete_unstable true true true - + make -j5 - check.wrap + check.base_unstable true true true @@ -2179,26 +2227,26 @@ true true - + make -j5 - check.base_unstable + check.unstable true true true - + make -j5 - testSpirit.run + wrap.testSpirit.run true true true - + make -j5 - testWrap.run + wrap.testWrap.run true true true diff --git a/CMakeLists.txt b/CMakeLists.txt index 983fece25..046209aa7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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") diff --git a/CppUnitLite/SimpleString.cpp b/CppUnitLite/SimpleString.cpp index 19dc7f258..190010d87 100644 --- a/CppUnitLite/SimpleString.cpp +++ b/CppUnitLite/SimpleString.cpp @@ -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_); diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index bd5c5295a..36f9ad8ca 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -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)"); diff --git a/gtsam.h b/gtsam.h index 665b9bb52..e80a37a0b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -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); }; diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 23b761b3b..105ff3a38 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -49,7 +49,7 @@ namespace gtsam { /* ************************************************************************* */ void DecisionTreeFactor::print(const string& s) const { - cout << s << ":\n"; + cout << s; IndexFactor::print("IndexFactor:"); Potentials::print("Potentials:"); } diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index c63e59517..537bb3e60 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -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 diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 8152ff726..3789389e4 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -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); } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index aa31bc9c0..d9da6e472 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -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 -#include -#include - -// 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 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 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 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 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& js, const std::vector& Gs, - const std::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& factors, - const std::vector& 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( - 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 upper-triangular part 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 upper-triangular part 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 splitEliminatedFactor(size_t nrFrontals); - - /** assert invariants */ - void assertInvariants() const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - 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 +#include +#include + +// 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 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 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 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 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& js, const std::vector& Gs, + const std::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& factors, + const std::vector& 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( + 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 upper-triangular part 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 upper-triangular part 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 splitEliminatedFactor(size_t nrFrontals); + + /** assert invariants */ + void assertInvariants() const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + 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_); + } + }; + +} + diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 30e8e19bb..1cdf9e1c7 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -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 diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 84090d666..3edb35a76 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -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::Create(*linear)->eliminate(eliminationMethod); + } + else if ( params_.isSequential() ) { + GaussianBayesNet::shared_ptr bn = EliminationTree::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"); } diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 8e2b20859..c81369299 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -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::Create(*linear)->eliminate(eliminationMethod)); - else + if ( params_.isMultifrontal() ) { + delta = GaussianJunctionTree(*linear).optimize(params_.getEliminationFunction()); + } + else if ( params_.isSequential() ) { + delta = gtsam::optimize(*EliminationTree::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 diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 30f0b14d2..42c38514c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -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; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index b9523b538..3701511fe 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -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::Create(dampedSystem)->eliminate(eliminationMethod)); - else + if ( params_.isMultifrontal() ) { + delta = GaussianJunctionTree(dampedSystem).optimize(params_.getEliminationFunction()); + } + else if ( params_.isSequential() ) { + delta = gtsam::optimize(*EliminationTree::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"); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 89b6de3dd..bf6527599 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -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& rekey_mapping) const { + shared_ptr new_factor = clone(); + for (size_t i=0; isize(); ++i) { + Key& cur_key = new_factor->keys()[i]; + std::map::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& new_keys) const { assert(new_keys.size() == this->keys().size()); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 2400ea996..d29e7282d 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -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 NonlinearFactorGraph::keys() const { + std::set keys; + BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + if(factor) + keys.insert(factor->begin(), factor->end()); } + return keys; +} - /* ************************************************************************* */ - std::set NonlinearFactorGraph::keys() const { - std::set 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 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 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& 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 diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 3caeec680..82b364ddf 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -87,6 +87,22 @@ namespace gtsam { boost::shared_ptr 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& rekey_mapping) const; + private: /** Serialization function */ diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index e2837a1ca..879b50677 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -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, diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index f17e01d49..387a550db 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -19,47 +19,59 @@ #pragma once #include +#include 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; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + boost::optional 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; + } } }; diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index f7563f8ff..9e9d66a7c 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -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}") diff --git a/gtsam_unstable/base/CMakeLists.txt b/gtsam_unstable/base/CMakeLists.txt index 7e455e9bf..0545cd4c9 100644 --- a/gtsam_unstable/base/CMakeLists.txt +++ b/gtsam_unstable/base/CMakeLists.txt @@ -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) diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 46efd4499..0cab961e1 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -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; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 04eeba953..46901fc7d 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -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; } diff --git a/gtsam_unstable/discrete/CMakeLists.txt b/gtsam_unstable/discrete/CMakeLists.txt index 3ba34d12b..edec85416 100644 --- a/gtsam_unstable/discrete/CMakeLists.txt +++ b/gtsam_unstable/discrete/CMakeLists.txt @@ -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) diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 4da2f440a..10c27d2bb 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -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((*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 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(f); + if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); + Constraint::shared_ptr reduced = constraint->partiallyApply(domains); if (print) reduced->print(); } #endif diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index e2e2a2251..ef4bbc17a 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -18,7 +18,7 @@ namespace gtsam { * A specialization of a DiscreteFactorGraph. * It knows about CSP-specific constraints and algorithms */ - class CSP: public FactorGraph { + class CSP: public DiscreteFactorGraph { public: /** A map from keys to values */ @@ -27,30 +27,10 @@ namespace gtsam { typedef boost::shared_ptr sharedValues; public: - /// Constructor - CSP() { - } - template - void add(const DiscreteKey& j, SOURCE table) { - DiscreteKeys keys; - keys.push_back(j); - push_back(boost::make_shared(keys, table)); - } - - template - 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(keys, table)); - } - - /** add shared discreteFactor immediately from arguments */ - template - void add(const DiscreteKeys& keys, SOURCE table) { - push_back(boost::make_shared(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 diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 678ba1580..d551b1f0b 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -105,7 +105,6 @@ namespace gtsam { /** Add student-specific constraints to the graph */ void Scheduler::addStudentSpecificConstraints(size_t i, boost::optional slot) { -#ifdef BROKEN bool debug = ISDEBUG("Scheduler::buildGraph"); assert(i domains; diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index 1bbac4777..583364ade 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -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 } diff --git a/gtsam_unstable/dynamics/CMakeLists.txt b/gtsam_unstable/dynamics/CMakeLists.txt index 66d0b9ac1..d0d45e0e4 100644 --- a/gtsam_unstable/dynamics/CMakeLists.txt +++ b/gtsam_unstable/dynamics/CMakeLists.txt @@ -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) diff --git a/gtsam_unstable/slam/CMakeLists.txt b/gtsam_unstable/slam/CMakeLists.txt index b0442044f..afe453279 100644 --- a/gtsam_unstable/slam/CMakeLists.txt +++ b/gtsam_unstable/slam/CMakeLists.txt @@ -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) diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 92e569038..4f3e7560f 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -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 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