From e57b569531d786afb3727380f5cee03ebc898627 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 24 May 2012 02:55:39 +0000 Subject: [PATCH 01/17] More descriptive error message when accessing a key that is not present - now tells what the key was --- gtsam/nonlinear/Ordering.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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, From 32871bfceb9bafbc04c60e0ccd148c39a94976f9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 24 May 2012 02:55:41 +0000 Subject: [PATCH 02/17] Fixed delete vs. delete[] in SimpleString - possible source of subtle errors --- CppUnitLite/SimpleString.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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_); From 47830cd56f337447821468b8ae270689c4d87e6e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 24 May 2012 16:05:01 +0000 Subject: [PATCH 03/17] Added clone and rekey facilities to nonlinear factor graph --- gtsam/nonlinear/NonlinearFactor.h | 18 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 208 +++++++++++++---------- gtsam/nonlinear/NonlinearFactorGraph.h | 16 ++ tests/testNonlinearFactorGraph.cpp | 37 ++++ 4 files changed, 186 insertions(+), 93 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 89b6de3dd..2b7311306 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -161,7 +161,23 @@ public: virtual shared_ptr clone() const =0; /** - * 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/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 Date: Thu, 24 May 2012 20:43:19 +0000 Subject: [PATCH 04/17] Added computeInformation function to GaussianFactor to properly compute information matrix including noise models, and using it to fix bug in Marginals where noise model was not being accounted for (only affects when hard constraints are used) --- gtsam/linear/GaussianFactor.h | 11 +++++++++++ gtsam/linear/HessianFactor.cpp | 5 +++++ gtsam/linear/HessianFactor.h | 17 +++++++++++++++++ gtsam/linear/JacobianFactor.cpp | 6 ++++++ gtsam/linear/JacobianFactor.h | 10 ++++++++++ gtsam/nonlinear/Marginals.cpp | 13 +++---------- 6 files changed, 52 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 3fdad19c6..e2537b7b4 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include @@ -88,6 +89,16 @@ namespace gtsam { /** Return the dimension of the variable pointed to by the given key iterator */ virtual size_t getDim(const_iterator variable) const = 0; + /** 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 = 0; + /** Clone a factor (make a deep copy) */ virtual GaussianFactor::shared_ptr clone() const = 0; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index b1f829005..aca473053 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -324,6 +324,11 @@ HessianFactor::constColumn HessianFactor::linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); } +/* ************************************************************************* */ +Matrix HessianFactor::computeInformation() const { + return info_.full().selfadjointView(); +} + /* ************************************************************************* */ double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d6ff23159..1454e7a36 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -269,6 +269,23 @@ namespace gtsam { /** 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; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index e2facec40..c7cdf6ce3 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -271,6 +271,12 @@ namespace gtsam { return 0.5 * weighted.dot(weighted); } + /* ************************************************************************* */ + Matrix JacobianFactor::computeInformation() const { + Matrix AbWhitened = Ab_.full(); + model_->WhitenInPlace(AbWhitened); + return AbWhitened.transpose() * AbWhitened; + } /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b5a2dd500..048d44981 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; /** Check if the factor contains no information, i.e. zero rows. This does * not necessarily mean that the factor involves no variables (to check for diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index ec837bc26..1abeb5ae9 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -69,16 +69,9 @@ Matrix Marginals::marginalInformation(Key variable) const { marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); // Get information matrix (only store upper-right triangle) - if(typeid(*marginalFactor) == typeid(JacobianFactor)) { - JacobianFactor::constABlock A = static_cast(*marginalFactor).getA(); - return A.transpose() * A; // Compute A'A - } else if(typeid(*marginalFactor) == typeid(HessianFactor)) { - const HessianFactor& hessian = static_cast(*marginalFactor); - const size_t dim = hessian.getDim(hessian.begin()); - return hessian.info().topLeftCorner(dim,dim).selfadjointView(); // Take the non-augmented part of the information matrix - } else { - throw runtime_error("Internal error: Marginals::marginalInformation expected either a JacobianFactor or HessianFactor"); - } + Matrix info = marginalFactor->computeInformation(); + const int dim = info.rows() - 1; + return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix } /* ************************************************************************* */ From 451dd5ef1b632209d15a7679f05569a5e647406c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 24 May 2012 20:43:22 +0000 Subject: [PATCH 05/17] Updated boost version requirement in README --- README | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README b/README index 45698af8e..c38c12306 100644 --- a/README +++ b/README @@ -51,7 +51,7 @@ Important Installation Notes 1) GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.40 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) - Cmake version 2.6 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher From 421a0725dd356cd7dc871c27b1049632065a30d2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 25 May 2012 14:12:28 +0000 Subject: [PATCH 06/17] Added a default implementation of clone() for nonlinear factor so not all subclasses need to implement clone(). Default implementation throws an exception. --- gtsam/nonlinear/NonlinearFactor.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 2b7311306..bf6527599 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -157,8 +157,14 @@ 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(); + } /** * Creates a shared_ptr clone of the factor with different keys using From 4ed447ca8e87e82f1df6d2b108a387b94e20839e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 May 2012 14:51:03 +0000 Subject: [PATCH 07/17] Fixed CSP, now a DiscreteFactorGraph again, uses dynamic_cast for Constraint-specific functionality :-( --- gtsam_unstable/discrete/AllDiff.cpp | 2 +- gtsam_unstable/discrete/BinaryAllDiff.h | 2 +- gtsam_unstable/discrete/CMakeLists.txt | 3 +- gtsam_unstable/discrete/CSP.cpp | 11 ++-- gtsam_unstable/discrete/CSP.h | 59 ++++++++------------ gtsam_unstable/discrete/SingleValue.cpp | 4 +- gtsam_unstable/discrete/tests/testCSP.cpp | 18 +++--- gtsam_unstable/discrete/tests/testSudoku.cpp | 10 ++-- 8 files changed, 52 insertions(+), 57 deletions(-) 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..707a67d84 100644 --- a/gtsam_unstable/discrete/CMakeLists.txt +++ b/gtsam_unstable/discrete/CMakeLists.txt @@ -18,7 +18,8 @@ set (discrete_full_libs # Exclude tests that don't work set (discrete_excluded_tests "${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp" -"${CMAKE_CURRENT_SOURCE_DIR}/tests/testCSP.cpp") +#"${CMAKE_CURRENT_SOURCE_DIR}/tests/testCSP.cpp" +) # Add all tests 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/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 855d62353..a626ecf13 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -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; } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 4d04afb92..c92cbe1f9 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -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 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 } From 14f119a787e9327a7b2289641e129de4b0b08f4f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 May 2012 15:09:59 +0000 Subject: [PATCH 08/17] Printing --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- gtsam/discrete/DecisionTreeFactor.h | 2 +- gtsam/discrete/DiscreteFactor.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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); } From de79924e7346f0b642c17e4cb03e752bb60f6b60 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 May 2012 15:10:13 +0000 Subject: [PATCH 09/17] Fixed discrete examples --- gtsam_unstable/discrete/CMakeLists.txt | 11 +++++------ gtsam_unstable/discrete/Scheduler.cpp | 11 +---------- 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/gtsam_unstable/discrete/CMakeLists.txt b/gtsam_unstable/discrete/CMakeLists.txt index 707a67d84..5d61db848 100644 --- a/gtsam_unstable/discrete/CMakeLists.txt +++ b/gtsam_unstable/discrete/CMakeLists.txt @@ -16,10 +16,9 @@ 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 @@ -27,8 +26,8 @@ gtsam_add_subdir_tests(discrete_unstable "${discrete_local_libs}" "${discrete_fu # 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/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 Date: Fri, 25 May 2012 15:26:30 +0000 Subject: [PATCH 11/17] reorg the nonlinear/linear parameters to accommodate the iterative solvers --- examples/PlanarSLAMSelfContained_advanced.cpp | 2 +- gtsam/nonlinear/DoglegOptimizer.cpp | 20 ++--- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 17 ++-- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 19 +++-- .../SuccessiveLinearizationOptimizer.h | 83 ++++++++++++------- tests/testNonlinearOptimizer.cpp | 4 +- 6 files changed, 87 insertions(+), 58 deletions(-) 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/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 84090d666..91b3c92fd 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_.isSPCG() ) { + 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..8d09b3edc 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_.isSPCG() ) { + throw runtime_error("todo: "); + } + else { throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination"); + } } // Maybe show output diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 1b19cfe75..3ad26cb3a 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -32,9 +32,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; @@ -67,12 +64,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_.isSPCG() ) { + 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/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index f17e01d49..2b3bfd3b6 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -24,42 +24,44 @@ 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, + SPCG }; - /** 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) - 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 SPCG: + std::cout << " linear solver type: SPCG\n"; + break; + default: + std::cout << " linear solver type: (invalid)\n"; + break; + } if(ordering) std::cout << " ordering: custom\n"; @@ -69,13 +71,32 @@ 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 isSPCG() const { + return (linearSolverType == SPCG); + } + + 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/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index a926b9e1b..eb6357fbd 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -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(); From efd94014b78001da4395c53c840887d1a56ec2e3 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 25 May 2012 16:12:55 +0000 Subject: [PATCH 12/17] Added check.unstable target - currently runs each unstable group separately, however --- .cproject | 280 +++++++++++++++---------- gtsam_unstable/CMakeLists.txt | 2 + gtsam_unstable/base/CMakeLists.txt | 2 +- gtsam_unstable/discrete/CMakeLists.txt | 1 + gtsam_unstable/dynamics/CMakeLists.txt | 2 +- gtsam_unstable/slam/CMakeLists.txt | 2 +- 6 files changed, 170 insertions(+), 119 deletions(-) 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/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index f7563f8ff..3fac03616 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 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/CMakeLists.txt b/gtsam_unstable/discrete/CMakeLists.txt index 5d61db848..edec85416 100644 --- a/gtsam_unstable/discrete/CMakeLists.txt +++ b/gtsam_unstable/discrete/CMakeLists.txt @@ -23,6 +23,7 @@ set (discrete_full_libs # 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 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) From 8f5dd13cf7cbe560a3b781759bc5485e4c4264c4 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 25 May 2012 17:55:02 +0000 Subject: [PATCH 13/17] Added access to the variable index in the iSAM2 solver --- gtsam/nonlinear/ISAM2.h | 3 +++ 1 file changed, 3 insertions(+) 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; From bce105ed585115663211f21c82b290be57d3dee4 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 25 May 2012 21:18:40 +0000 Subject: [PATCH 14/17] Removed nonexistent function from matlab interface --- gtsam.h | 1 - 1 file changed, 1 deletion(-) 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); }; From 0d597082f23af78b3de48368becf396f5087afc5 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 25 May 2012 21:18:41 +0000 Subject: [PATCH 15/17] Added optional target to build the matlab toolbox as a part of the gtsam build process --- CMakeLists.txt | 3 +++ wrap/CMakeLists.txt | 11 +++++++++++ 2 files changed, 14 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d2acb464..7819cfdc8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,6 +61,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/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 8feb5779b..36415df35 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -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) From b0c91d2fcf8bd8e315082ba6769be4da2f5ca340 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sun, 27 May 2012 18:26:25 +0000 Subject: [PATCH 16/17] --- gtsam/nonlinear/DoglegOptimizer.cpp | 2 +- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 2 +- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 2 +- .../SuccessiveLinearizationOptimizer.h | 24 +++++++++++++++---- 4 files changed, 22 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 91b3c92fd..3edb35a76 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -48,7 +48,7 @@ void DoglegOptimizer::iterate(void) { 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 if ( params_.isSPCG() ) { + else if ( params_.isCG() ) { throw runtime_error("todo: "); } else { diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 8d09b3edc..c81369299 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -41,7 +41,7 @@ void GaussNewtonOptimizer::iterate() { else if ( params_.isSequential() ) { delta = gtsam::optimize(*EliminationTree::Create(*linear)->eliminate(params_.getEliminationFunction())); } - else if ( params_.isSPCG() ) { + else if ( params_.isCG() ) { throw runtime_error("todo: "); } else { diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 3ad26cb3a..f1f5ab1df 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -70,7 +70,7 @@ void LevenbergMarquardtOptimizer::iterate() { else if ( params_.isSequential() ) { delta = gtsam::optimize(*EliminationTree::Create(dampedSystem)->eliminate(params_.getEliminationFunction())); } - else if ( params_.isSPCG() ) { + else if ( params_.isCG() ) { throw runtime_error("todo: "); } else { diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index 2b3bfd3b6..387a550db 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -19,6 +19,7 @@ #pragma once #include +#include namespace gtsam { @@ -30,11 +31,14 @@ public: MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - SPCG + CHOLMOD, /* Experimental Flag */ + PCG, /* Experimental Flag */ + LSPCG /* Experimental Flag */ }; 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() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} @@ -55,8 +59,14 @@ public: case SEQUENTIAL_QR: std::cout << " linear solver type: SEQUENTIAL QR\n"; break; - case SPCG: - std::cout << " linear solver type: SPCG\n"; + 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"; @@ -79,8 +89,12 @@ public: return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR); } - inline bool isSPCG() const { - return (linearSolverType == SPCG); + inline bool isCholmod() const { + return (linearSolverType == CHOLMOD); + } + + inline bool isCG() const { + return (linearSolverType == PCG || linearSolverType == LSPCG); } GaussianFactorGraph::Eliminate getEliminationFunction() { From 3c694e25f71a12ed43c8444150aae80b9c431258 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sun, 27 May 2012 20:05:42 +0000 Subject: [PATCH 17/17] Added experimental target for building unstable matlab toolbox --- gtsam_unstable/CMakeLists.txt | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 3fac03616..9e9d66a7c 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -88,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}")