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