diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp new file mode 100644 index 000000000..4bbaac3ef --- /dev/null +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample.cpp + * @brief This file is to compare the ordering performance for COLAMD vs METIS. + * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file. + * @author Frank Dellaert, Zhaoyang Lv + */ + +// For an explanation of headers, see SFMExample.cpp +#include +#include +#include +#include +#include +#include +#include // for loading BAL datasets ! + +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::C; +using symbol_shorthand::P; + +// We will be using a projection factor that ties a SFM_Camera to a 3D point. +// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +// and has a total of 9 free parameters +typedef GeneralSFMFactor MyFactor; + +/* ************************************************************************* */ +int main (int argc, char* argv[]) { + + // Find default file, but if an argument is given, try loading a file + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + if (argc>1) filename = string(argv[1]); + + // Load the SfM data from file + SfM_data mydata; + readBAL(filename, mydata); + cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // We share *one* noiseModel between all projection factors + noiseModel::Isotropic::shared_ptr noise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Add measurements to the factor graph + size_t j = 0; + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + } + j += 1; + } + + // Add a prior on pose x1. This indirectly specifies where the origin is. + // and a prior on the position of the first landmark to fix the scale + graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); + graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + + // Create initial estimate + Values initial; + size_t i = 0; j = 0; + BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); + + /** --------------- COMPARISON -----------------------**/ + /** ----------------------------------------------------**/ + + LevenbergMarquardtParams params_using_COLAMD, params_using_METIS; + try { + params_using_METIS.setVerbosity("ERROR"); + gttic_(METIS_ORDERING); + params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); + gttoc_(METIS_ORDERING); + + params_using_COLAMD.setVerbosity("ERROR"); + gttic_(COLAMD_ORDERING); + params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); + gttoc_(COLAMD_ORDERING); + } catch (exception& e) { + cout << e.what(); + } + + // expect they have different ordering results + if(params_using_COLAMD.ordering == params_using_METIS.ordering) { + cout << "COLAMD and METIS produce the same ordering. " + << "Problem here!!!" << endl; + } + + /* Optimize the graph with METIS and COLAMD and time the results */ + + Values result_METIS, result_COLAMD; + try { + gttic_(OPTIMIZE_WITH_METIS); + LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS); + result_METIS = lm_METIS.optimize(); + gttoc_(OPTIMIZE_WITH_METIS); + + gttic_(OPTIMIZE_WITH_COLAMD); + LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD); + result_COLAMD = lm_COLAMD.optimize(); + gttoc_(OPTIMIZE_WITH_COLAMD); + } catch (exception& e) { + cout << e.what(); + } + + + { // printing the result + + cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl; + cout << "METIS final error: " << graph.error(result_METIS) << endl; + + cout << endl << endl; + + cout << "Time comparison by solving " << filename << " results:" << endl; + cout << boost::format("%1% point tracks and %2% cameras\n") \ + % mydata.number_tracks() % mydata.number_cameras() \ + << endl; + + tictoc_print_(); + } + + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index f8f1411a3..068846884 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -576,7 +576,7 @@ void runStats() { cout << "Gathering statistics..." << endl; GaussianFactorGraph linear = *datasetMeasurements.linearize(initial); - GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear))); + GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear))); treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt); ofstream file; diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 5e261e200..b4fc3b3a6 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -55,9 +55,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::METIS) - return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); + return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType); + else + return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -93,9 +93,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::METIS) - return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType); + return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); else - return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); + return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -125,7 +125,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Compute full ordering - Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -163,7 +163,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Compute full ordering - Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -216,7 +216,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -275,7 +275,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -301,7 +301,7 @@ namespace gtsam { if(variableIndex) { // Compute a total ordering for all variables - Ordering totalOrdering = Ordering::colamdConstrainedLast(*variableIndex, variables); + Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables); // Split out the part for the marginalized variables Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 5fb5fbdb1..f5431db3d 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -128,7 +128,8 @@ namespace gtsam { OptionalOrderingType orderingType = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not - * provided, the ordering provided by COLAMD will be used. + * provided, the ordering will be computed using either COLAMD or METIS, dependeing on + * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) * * Example - Full Cholesky elimination in COLAMD order: * \code diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 8be01ac5f..99f668591 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -46,7 +46,7 @@ namespace gtsam { const VariableIndex varIndex(factors); const KeySet newFactorKeys = newFactors.keys(); const Ordering constrainedOrdering = - Ordering::colamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); + Ordering::ColamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end()); this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end()); diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 5ae25d543..9f4a81d05 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -29,242 +29,236 @@ using namespace std; namespace gtsam { - /* ************************************************************************* */ - FastMap Ordering::invert() const - { - FastMap inverted; - for(size_t pos = 0; pos < this->size(); ++pos) - inverted.insert(make_pair((*this)[pos], pos)); - return inverted; +/* ************************************************************************* */ +FastMap Ordering::invert() const { + FastMap inverted; + for (size_t pos = 0; pos < this->size(); ++pos) + inverted.insert(make_pair((*this)[pos], pos)); + return inverted; +} + +/* ************************************************************************* */ +Ordering Ordering::Colamd(const VariableIndex& variableIndex) { + // Call constrained version with all groups set to zero + vector dummy_groups(variableIndex.size(), 0); + return Ordering::ColamdConstrained(variableIndex, dummy_groups); +} + +/* ************************************************************************* */ +Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, + std::vector& cmember) { + gttic(Ordering_COLAMDConstrained); + + gttic(Prepare); + size_t nEntries = variableIndex.nEntries(), nFactors = + variableIndex.nFactors(), nVars = variableIndex.size(); + // Convert to compressed column major format colamd wants it in (== MATLAB format!) + size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, + (int) nVars); /* colamd arg 3: size of the array A */ + vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ + vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + + // Fill in input data for COLAMD + p[0] = 0; + int count = 0; + vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order + size_t index = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { + // Arrange factor indices into COLAMD format + const VariableIndex::Factors& column = key_factors.second; + size_t lastFactorId = numeric_limits::max(); + BOOST_FOREACH(size_t factorIndex, column) { + if (lastFactorId != numeric_limits::max()) + assert(factorIndex > lastFactorId); + A[count++] = (int) factorIndex; // copy sparse column + } + p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 + // Store key in array and increment index + keys[index] = key_factors.first; + ++index; } - /* ************************************************************************* */ - Ordering Ordering::colamd(const VariableIndex& variableIndex) - { - // Call constrained version with all groups set to zero - vector dummy_groups(variableIndex.size(), 0); - return Ordering::colamdConstrained(variableIndex, dummy_groups); + assert((size_t)count == variableIndex.nEntries()); + + //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + double knobs[CCOLAMD_KNOBS]; + ccolamd_set_defaults(knobs); + knobs[CCOLAMD_DENSE_ROW] = -1; + knobs[CCOLAMD_DENSE_COL] = -1; + + int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + + gttoc(Prepare); + + // call colamd, result will be in p + /* returns (1) if successful, (0) otherwise*/ + if (nVars > 0) { + gttic(ccolamd); + int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0], + knobs, stats, &cmember[0]); + if (rv != 1) + throw runtime_error( + (boost::format("ccolamd failed with return value %1%") % rv).str()); } - /* ************************************************************************* */ - Ordering Ordering::colamdConstrained( - const VariableIndex& variableIndex, std::vector& cmember) - { - gttic(Ordering_COLAMDConstrained); + // ccolamd_report(stats); - gttic(Prepare); - size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); - // Convert to compressed column major format colamd wants it in (== MATLAB format!) - size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */ - vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ - vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + gttic(Fill_Ordering); + // Convert elimination ordering in p to an ordering + Ordering result; + result.resize(nVars); + for (size_t j = 0; j < nVars; ++j) + result[j] = keys[p[j]]; + gttoc(Fill_Ordering); - // Fill in input data for COLAMD - p[0] = 0; - int count = 0; - vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order - size_t index = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { - // Arrange factor indices into COLAMD format - const VariableIndex::Factors& column = key_factors.second; - size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(size_t factorIndex, column) { - if(lastFactorId != numeric_limits::max()) - assert(factorIndex > lastFactorId); - A[count++] = (int)factorIndex; // copy sparse column - } - p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 - // Store key in array and increment index - keys[index] = key_factors.first; - ++ index; - } + return result; +} - assert((size_t)count == variableIndex.nEntries()); +/* ************************************************************************* */ +Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex, + const std::vector& constrainLast, bool forceOrder) { + gttic(Ordering_COLAMDConstrainedLast); - //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ - double knobs[CCOLAMD_KNOBS]; - ccolamd_set_defaults(knobs); - knobs[CCOLAMD_DENSE_ROW]=-1; - knobs[CCOLAMD_DENSE_COL]=-1; + size_t n = variableIndex.size(); + std::vector cmember(n, 0); - int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - gttoc(Prepare); + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + int group = (constrainLast.size() != n ? 1 : 0); + BOOST_FOREACH(Key key, constrainLast) { + cmember[keyIndices.at(key)] = group; + if (forceOrder) + ++group; + } - // call colamd, result will be in p - /* returns (1) if successful, (0) otherwise*/ - if(nVars > 0) { - gttic(ccolamd); - int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]); - if(rv != 1) - throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str()); - } + return Ordering::ColamdConstrained(variableIndex, cmember); +} - // ccolamd_report(stats); +/* ************************************************************************* */ +Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, + const std::vector& constrainFirst, bool forceOrder) { + gttic(Ordering_COLAMDConstrainedFirst); - gttic(Fill_Ordering); - // Convert elimination ordering in p to an ordering - Ordering result; - result.resize(nVars); - for(size_t j = 0; j < nVars; ++j) - result[j] = keys[p[j]]; - gttoc(Fill_Ordering); + const int none = -1; + size_t n = variableIndex.size(); + std::vector cmember(n, none); + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); + + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + int group = 0; + BOOST_FOREACH(Key key, constrainFirst) { + cmember[keyIndices.at(key)] = group; + if (forceOrder) + ++group; + } + + if (!forceOrder && !constrainFirst.empty()) + ++group; + BOOST_FOREACH(int& c, cmember) + if (c == none) + c = group; + + return Ordering::ColamdConstrained(variableIndex, cmember); +} + +/* ************************************************************************* */ +Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, + const FastMap& groups) { + gttic(Ordering_COLAMDConstrained); + size_t n = variableIndex.size(); + std::vector cmember(n, 0); + + // Build a mapping to look up sorted Key indices by Key + FastMap keyIndices; + size_t j = 0; + BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); + + // Assign groups + typedef FastMap::value_type key_group; + BOOST_FOREACH(const key_group& p, groups) { + // FIXME: check that no groups are skipped + cmember[keyIndices.at(p.first)] = p.second; + } + + return Ordering::ColamdConstrained(variableIndex, cmember); +} + +/* ************************************************************************* */ +Ordering Ordering::Metis(const MetisIndex& met) { + gttic(Ordering_METIS); + + vector xadj = met.xadj(); + vector adj = met.adj(); + vector perm, iperm; + + idx_t size = met.nValues(); + for (idx_t i = 0; i < size; i++) { + perm.push_back(0); + iperm.push_back(0); + } + + int outputError; + + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], + &iperm[0]); + Ordering result; + + if (outputError != METIS_OK) { + std::cout << "METIS failed during Nested Dissection ordering!\n"; return result; } - /* ************************************************************************* */ - Ordering Ordering::colamdConstrainedLast( - const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) - { - gttic(Ordering_COLAMDConstrainedLast); - - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - int group = (constrainLast.size() != n ? 1 : 0); - BOOST_FOREACH(Key key, constrainLast) { - cmember[keyIndices.at(key)] = group; - if(forceOrder) - ++ group; - } - - return Ordering::colamdConstrained(variableIndex, cmember); + result.resize(size); + for (size_t j = 0; j < (size_t) size; ++j) { + // We have to add the minKey value back to obtain the original key in the Values + result[j] = met.intToKey(perm[j]); } + return result; +} - /* ************************************************************************* */ - Ordering Ordering::colamdConstrainedFirst( - const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) - { - gttic(Ordering_COLAMDConstrainedFirst); - - const int none = -1; - size_t n = variableIndex.size(); - std::vector cmember(n, none); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - int group = 0; - BOOST_FOREACH(Key key, constrainFirst) { - cmember[keyIndices.at(key)] = group; - if(forceOrder) - ++ group; - } - - if(!forceOrder && !constrainFirst.empty()) - ++ group; - BOOST_FOREACH(int& c, cmember) - if(c == none) - c = group; - - return Ordering::colamdConstrained(variableIndex, cmember); - } - - /* ************************************************************************* */ - Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, - const FastMap& groups) - { - gttic(Ordering_COLAMDConstrained); - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - // Build a mapping to look up sorted Key indices by Key - FastMap keyIndices; - size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) - keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); - - // Assign groups - typedef FastMap::value_type key_group; - BOOST_FOREACH(const key_group& p, groups) { - // FIXME: check that no groups are skipped - cmember[keyIndices.at(p.first)] = p.second; - } - - return Ordering::colamdConstrained(variableIndex, cmember); - } - - - /* ************************************************************************* */ - Ordering Ordering::metis(const MetisIndex& met) - { - gttic(Ordering_METIS); - - vector xadj = met.xadj(); - vector adj = met.adj(); - vector perm, iperm; - - idx_t size = met.nValues(); - for (idx_t i = 0; i < size; i++) - { - perm.push_back(0); - iperm.push_back(0); - } - - int outputError; - - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); - Ordering result; - - if (outputError != METIS_OK) - { - std::cout << "METIS failed during Nested Dissection ordering!\n"; - return result; - } - - result.resize(size); - for (size_t j = 0; j < (size_t)size; ++j){ - // We have to add the minKey value back to obtain the original key in the Values - result[j] = met.intToKey(perm[j]); - } - return result; - } - - /* ************************************************************************* */ - void Ordering::print(const std::string& str, const KeyFormatter& keyFormatter) const - { - cout << str; - // Print ordering in index order - // Print the ordering with varsPerLine ordering entries printed on each line, - // for compactness. - static const size_t varsPerLine = 10; - bool endedOnNewline = false; - for(size_t i = 0; i < size(); ++i) { - if(i % varsPerLine == 0) - cout << "Position " << i << ": "; - if(i % varsPerLine != 0) - cout << ", "; - cout << keyFormatter(at(i)); - if(i % varsPerLine == varsPerLine - 1) { - cout << "\n"; - endedOnNewline = true; - } else { - endedOnNewline = false; - } - } - if(!endedOnNewline) +/* ************************************************************************* */ +void Ordering::print(const std::string& str, + const KeyFormatter& keyFormatter) const { + cout << str; + // Print ordering in index order + // Print the ordering with varsPerLine ordering entries printed on each line, + // for compactness. + static const size_t varsPerLine = 10; + bool endedOnNewline = false; + for (size_t i = 0; i < size(); ++i) { + if (i % varsPerLine == 0) + cout << "Position " << i << ": "; + if (i % varsPerLine != 0) + cout << ", "; + cout << keyFormatter(at(i)); + if (i % varsPerLine == varsPerLine - 1) { cout << "\n"; - cout.flush(); + endedOnNewline = true; + } else { + endedOnNewline = false; + } } + if (!endedOnNewline) + cout << "\n"; + cout.flush(); +} - /* ************************************************************************* */ - bool Ordering::equals(const Ordering& other, double tol) const - { - return (*this) == other; - } +/* ************************************************************************* */ +bool Ordering::equals(const Ordering& other, double tol) const { + return (*this) == other; +} } diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 2d95442de..8af2649c5 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -30,170 +30,212 @@ namespace gtsam { - class Ordering : public std::vector { - protected: - typedef std::vector Base; +class Ordering: public std::vector { +protected: + typedef std::vector Base; - public: +public: - /// Type of ordering to use - enum OrderingType { - COLAMD, METIS, CUSTOM - }; - - typedef Ordering This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - - /// Create an empty ordering - GTSAM_EXPORT Ordering() {} - - /// Create from a container - template - explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {} - - /// Create an ordering using iterators over keys - template - Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {} - - /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling - /// push_back. - boost::assign::list_inserter > - operator+=(Key key) { - return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*this))(key); - } - - /// Invert (not reverse) the ordering - returns a map from key to order position - FastMap invert() const; - - /// @name Fill-reducing Orderings @{ - - /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on - /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, - /// it is faster to use COLAMD(const VariableIndex&) - template - static Ordering colamd(const FactorGraph& graph) { - return colamd(VariableIndex(graph)); } - - /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. - static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); - - /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details - /// for note on performance). This internally builds a VariableIndex so if you already have a - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains - /// the variables in \c constrainLast to the end of the ordering, and orders all other variables - /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c - /// constrainLast will be ordered in the same order specified in the vector \c - /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be - /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - template - static Ordering colamdConstrainedLast(const FactorGraph& graph, - const std::vector& constrainLast, bool forceOrder = false) { - return colamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } - - /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This - /// function constrains the variables in \c constrainLast to the end of the ordering, and orders - /// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the - /// variables in \c constrainLast will be ordered in the same order specified in the vector - /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be - /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - static GTSAM_EXPORT Ordering colamdConstrainedLast(const VariableIndex& variableIndex, - const std::vector& constrainLast, bool forceOrder = false); - - /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details - /// for note on performance). This internally builds a VariableIndex so if you already have a - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains - /// the variables in \c constrainLast to the end of the ordering, and orders all other variables - /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c - /// constrainFirst will be ordered in the same order specified in the vector \c - /// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be - /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - template - static Ordering colamdConstrainedFirst(const FactorGraph& graph, - const std::vector& constrainFirst, bool forceOrder = false) { - return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } - - /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This - /// function constrains the variables in \c constrainFirst to the front of the ordering, and - /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the - /// variables in \c constrainFirst will be ordered in the same order specified in the - /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c - /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to - /// reduce fill-in as well. - static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex, - const std::vector& constrainFirst, bool forceOrder = false); - - /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details - /// for note on performance). This internally builds a VariableIndex so if you already have a - /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group - /// for each variable should be specified in \c groups, and each group of variables will appear - /// in the ordering in group index order. \c groups should be a map from Key to group index. - /// The group indices used should be consecutive starting at 0, but may appear in \c groups in - /// arbitrary order. Any variables not present in \c groups will be assigned to group 0. This - /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the - /// CCOLAMD documentation for more information. - template - static Ordering colamdConstrained(const FactorGraph& graph, - const FastMap& groups) { - return colamdConstrained(VariableIndex(graph), groups); } - - /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this - /// function, a group for each variable should be specified in \c groups, and each group of - /// variables will appear in the ordering in group index order. \c groups should be a map from - /// Key to group index. The group indices used should be consecutive starting at 0, but may - /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be - /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the - /// supplied indices, see the CCOLAMD documentation for more information. - static GTSAM_EXPORT Ordering colamdConstrained(const VariableIndex& variableIndex, - const FastMap& groups); - - /// Return a natural Ordering. Typically used by iterative solvers - template - static Ordering Natural(const FactorGraph &fg) { - KeySet src = fg.keys(); - std::vector keys(src.begin(), src.end()); - std::stable_sort(keys.begin(), keys.end()); - return Ordering(keys); - } - - /// METIS Formatting function - template - static GTSAM_EXPORT void CSRFormat(std::vector& xadj, std::vector& adj, const FactorGraph& graph); - - /// Compute an ordering determined by METIS from a VariableIndex - static GTSAM_EXPORT Ordering metis(const MetisIndex& met); - - template - static Ordering metis(const FactorGraph& graph) - { - return metis(MetisIndex(graph)); - } - - /// @} - - /// @name Testable @{ - - GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const; - - /// @} - - private: - /// Internal COLAMD function - static GTSAM_EXPORT Ordering colamdConstrained( - const VariableIndex& variableIndex, std::vector& cmember); - - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } + /// Type of ordering to use + enum OrderingType { + COLAMD, METIS, NATURAL, CUSTOM }; - /// traits - template<> struct traits : public Testable {}; + typedef Ordering This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + + /// Create an empty ordering + GTSAM_EXPORT + Ordering() { + } + + /// Create from a container + template + explicit Ordering(const KEYS& keys) : + Base(keys.begin(), keys.end()) { + } + + /// Create an ordering using iterators over keys + template + Ordering(ITERATOR firstKey, ITERATOR lastKey) : + Base(firstKey, lastKey) { + } + + /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling + /// push_back. + boost::assign::list_inserter > operator+=( + Key key) { + return boost::assign::make_list_inserter( + boost::assign_detail::call_push_back(*this))(key); + } + + /// Invert (not reverse) the ordering - returns a map from key to order position + FastMap invert() const; + + /// @name Fill-reducing Orderings @{ + + /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on + /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, + /// it is faster to use COLAMD(const VariableIndex&) + template + static Ordering Colamd(const FactorGraph& graph) { + return Colamd(VariableIndex(graph)); + } + + /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. + static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex); + + /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details + /// for note on performance). This internally builds a VariableIndex so if you already have a + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains + /// the variables in \c constrainLast to the end of the ordering, and orders all other variables + /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c + /// constrainLast will be ordered in the same order specified in the vector \c + /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be + /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + template + static Ordering ColamdConstrainedLast(const FactorGraph& graph, + const std::vector& constrainLast, bool forceOrder = false) { + return ColamdConstrainedLast(VariableIndex(graph), constrainLast, + forceOrder); + } + + /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This + /// function constrains the variables in \c constrainLast to the end of the ordering, and orders + /// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the + /// variables in \c constrainLast will be ordered in the same order specified in the vector + /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be + /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + static GTSAM_EXPORT Ordering ColamdConstrainedLast( + const VariableIndex& variableIndex, const std::vector& constrainLast, + bool forceOrder = false); + + /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details + /// for note on performance). This internally builds a VariableIndex so if you already have a + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains + /// the variables in \c constrainLast to the end of the ordering, and orders all other variables + /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c + /// constrainFirst will be ordered in the same order specified in the vector \c + /// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be + /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + template + static Ordering ColamdConstrainedFirst(const FactorGraph& graph, + const std::vector& constrainFirst, bool forceOrder = false) { + return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, + forceOrder); + } + + /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This + /// function constrains the variables in \c constrainFirst to the front of the ordering, and + /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the + /// variables in \c constrainFirst will be ordered in the same order specified in the + /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c + /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to + /// reduce fill-in as well. + static GTSAM_EXPORT Ordering ColamdConstrainedFirst( + const VariableIndex& variableIndex, + const std::vector& constrainFirst, bool forceOrder = false); + + /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details + /// for note on performance). This internally builds a VariableIndex so if you already have a + /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group + /// for each variable should be specified in \c groups, and each group of variables will appear + /// in the ordering in group index order. \c groups should be a map from Key to group index. + /// The group indices used should be consecutive starting at 0, but may appear in \c groups in + /// arbitrary order. Any variables not present in \c groups will be assigned to group 0. This + /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the + /// CCOLAMD documentation for more information. + template + static Ordering ColamdConstrained(const FactorGraph& graph, + const FastMap& groups) { + return ColamdConstrained(VariableIndex(graph), groups); + } + + /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this + /// function, a group for each variable should be specified in \c groups, and each group of + /// variables will appear in the ordering in group index order. \c groups should be a map from + /// Key to group index. The group indices used should be consecutive starting at 0, but may + /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be + /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the + /// supplied indices, see the CCOLAMD documentation for more information. + static GTSAM_EXPORT Ordering ColamdConstrained( + const VariableIndex& variableIndex, const FastMap& groups); + + /// Return a natural Ordering. Typically used by iterative solvers + template + static Ordering Natural(const FactorGraph &fg) { + KeySet src = fg.keys(); + std::vector keys(src.begin(), src.end()); + std::stable_sort(keys.begin(), keys.end()); + return Ordering(keys); + } + + /// METIS Formatting function + template + static GTSAM_EXPORT void CSRFormat(std::vector& xadj, + std::vector& adj, const FactorGraph& graph); + + /// Compute an ordering determined by METIS from a VariableIndex + static GTSAM_EXPORT Ordering Metis(const MetisIndex& met); + + template + static Ordering Metis(const FactorGraph& graph) { + return Metis(MetisIndex(graph)); + } + + /// @} + + /// @name Named Constructors @{ + + template + static Ordering Create(OrderingType orderingType, + const FactorGraph& graph) { + + switch (orderingType) { + case COLAMD: + return Colamd(graph); + case METIS: + return Metis(graph); + case NATURAL: + return Natural(graph); + case CUSTOM: + throw std::runtime_error( + "Ordering::Create error: called with CUSTOM ordering type."); + default: + throw std::runtime_error( + "Ordering::Create error: called with unknown ordering type."); + } + } + + /// @} + + /// @name Testable @{ + + GTSAM_EXPORT + void print(const std::string& str = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + + GTSAM_EXPORT + bool equals(const Ordering& other, double tol = 1e-9) const; + + /// @} + +private: + /// Internal COLAMD function + static GTSAM_EXPORT Ordering ColamdConstrained( + const VariableIndex& variableIndex, std::vector& cmember); + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +/// traits +template<> struct traits : public Testable { +}; } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 013f569e0..d789da9b0 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -28,45 +28,47 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +namespace example { +SymbolicFactorGraph symbolicChain() { + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(4, 5); + return sfg; +} +} /* ************************************************************************* */ TEST(Ordering, constrained_ordering) { - SymbolicFactorGraph sfg; // create graph with wanted variable set = 2, 4 - sfg.push_factor(0,1); - sfg.push_factor(1,2); - sfg.push_factor(2,3); - sfg.push_factor(3,4); - sfg.push_factor(4,5); + SymbolicFactorGraph sfg = example::symbolicChain(); // unconstrained version - Ordering actUnconstrained = Ordering::colamd(sfg); + Ordering actUnconstrained = Ordering::Colamd(sfg); Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5)); EXPECT(assert_equal(expUnconstrained, actUnconstrained)); // constrained version - push one set to the end - Ordering actConstrained = Ordering::colamdConstrainedLast(sfg, list_of(2)(4)); + Ordering actConstrained = Ordering::ColamdConstrainedLast(sfg, list_of(2)(4)); Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2)); EXPECT(assert_equal(expConstrained, actConstrained)); // constrained version - push one set to the start - Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4)); + Ordering actConstrained2 = Ordering::ColamdConstrainedFirst(sfg, + list_of(2)(4)); Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); EXPECT(assert_equal(expConstrained2, actConstrained2)); } /* ************************************************************************* */ TEST(Ordering, grouped_constrained_ordering) { - SymbolicFactorGraph sfg; // create graph with constrained groups: // 1: 2, 4 // 2: 5 - sfg.push_factor(0,1); - sfg.push_factor(1,2); - sfg.push_factor(2,3); - sfg.push_factor(3,4); - sfg.push_factor(4,5); + SymbolicFactorGraph sfg = example::symbolicChain(); // constrained version - push one set to the end FastMap constraints; @@ -74,50 +76,48 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[4] = 1; constraints[5] = 2; - Ordering actConstrained = Ordering::colamdConstrained(sfg, constraints); + Ordering actConstrained = Ordering::ColamdConstrained(sfg, constraints); Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5); EXPECT(assert_equal(expConstrained, actConstrained)); } /* ************************************************************************* */ TEST(Ordering, csr_format) { - // Example in METIS manual - SymbolicFactorGraph sfg; - sfg.push_factor(0, 1); - sfg.push_factor(1, 2); - sfg.push_factor(2, 3); - sfg.push_factor(3, 4); - sfg.push_factor(5, 6); - sfg.push_factor(6, 7); - sfg.push_factor(7, 8); - sfg.push_factor(8, 9); - sfg.push_factor(10, 11); - sfg.push_factor(11, 12); - sfg.push_factor(12, 13); - sfg.push_factor(13, 14); + // Example in METIS manual + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(5, 6); + sfg.push_factor(6, 7); + sfg.push_factor(7, 8); + sfg.push_factor(8, 9); + sfg.push_factor(10, 11); + sfg.push_factor(11, 12); + sfg.push_factor(12, 13); + sfg.push_factor(13, 14); - sfg.push_factor(0, 5); - sfg.push_factor(5, 10); - sfg.push_factor(1, 6); - sfg.push_factor(6, 11); - sfg.push_factor(2, 7); - sfg.push_factor(7, 12); - sfg.push_factor(3, 8); - sfg.push_factor(8, 13); - sfg.push_factor(4, 9); - sfg.push_factor(9, 14); + sfg.push_factor(0, 5); + sfg.push_factor(5, 10); + sfg.push_factor(1, 6); + sfg.push_factor(6, 11); + sfg.push_factor(2, 7); + sfg.push_factor(7, 12); + sfg.push_factor(3, 8); + sfg.push_factor(8, 13); + sfg.push_factor(4, 9); + sfg.push_factor(9, 14); - MetisIndex mi(sfg); + MetisIndex mi(sfg); - vector xadjExpected, adjExpected; - xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; - adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, - 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, - 13, 8, 12, 14, 9, 13 ; + vector xadjExpected, adjExpected; + xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; + adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, 13, 8, 12, 14, 9, 13; - EXPECT(xadjExpected == mi.xadj()); - EXPECT(adjExpected.size() == mi.adj().size()); - EXPECT(adjExpected == mi.adj()); + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); } /* ************************************************************************* */ @@ -135,12 +135,11 @@ TEST(Ordering, csr_format_2) { vector xadjExpected, adjExpected; xadjExpected += 0, 1, 4, 6, 8, 10; - adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - } /* ************************************************************************* */ @@ -170,7 +169,6 @@ TEST(Ordering, csr_format_3) { EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - } /* ************************************************************************* */ @@ -197,7 +195,7 @@ TEST(Ordering, csr_format_4) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - Ordering metOrder = Ordering::metis(sfg); + Ordering metOrder = Ordering::Metis(sfg); // Test different symbol types sfg.push_factor(Symbol('l', 1)); @@ -206,8 +204,7 @@ TEST(Ordering, csr_format_4) { sfg.push_factor(Symbol('x', 3), Symbol('l', 1)); sfg.push_factor(Symbol('x', 4), Symbol('l', 1)); - Ordering metOrder2 = Ordering::metis(sfg); - + Ordering metOrder2 = Ordering::Metis(sfg); } /* ************************************************************************* */ @@ -229,8 +226,77 @@ TEST(Ordering, metis) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - Ordering metis = Ordering::metis(sfg); + Ordering metis = Ordering::Metis(sfg); +} + +/* ************************************************************************* */ +TEST(Ordering, MetisLoop) { + + // create linear graph + SymbolicFactorGraph sfg = example::symbolicChain(); + + // add loop closure + sfg.push_factor(0, 5); + + // METIS +#if !defined(__APPLE__) + { + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // - P( 0 4 1) + // | - P( 2 | 4 1) + // | | - P( 3 | 4 2) + // | - P( 5 | 0 1) + Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1)); + EXPECT(assert_equal(expected, actual)); + } +#else + { + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + // - P( 1 0 3) + // | - P( 4 | 0 3) + // | | - P( 5 | 0 4) + // | - P( 2 | 1 3) + Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); + EXPECT(assert_equal(expected, actual)); + } +#endif +} + +/* ************************************************************************* */ +TEST(Ordering, Create) { + + // create chain graph + SymbolicFactorGraph sfg = example::symbolicChain(); + + // COLAMD + { + //- P( 4 5) + //| - P( 3 | 4) + //| | - P( 2 | 3) + //| | | - P( 1 | 2) + //| | | | - P( 0 | 1) + Ordering actual = Ordering::Create(Ordering::COLAMD, sfg); + Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5)); + EXPECT(assert_equal(expected, actual)); + } + + // METIS + { + Ordering actual = Ordering::Create(Ordering::METIS, sfg); + //- P( 1 0 2) + //| - P( 3 4 | 2) + //| | - P( 5 | 4) + Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); + EXPECT(assert_equal(expected, actual)); + } + + // CUSTOM + CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, sfg), runtime_error); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index e148bd65d..a91515e9c 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -95,8 +95,8 @@ void DoglegOptimizer::iterate(void) { /* ************************************************************************* */ DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const { - if(!params.ordering) - params.ordering = Ordering::colamd(graph); + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 7a115e1c4..e420567ec 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -46,10 +46,9 @@ void GaussNewtonOptimizer::iterate() { /* ************************************************************************* */ GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering( - GaussNewtonParams params, const NonlinearFactorGraph& graph) const -{ - if(!params.ordering) - params.ordering = Ordering::colamd(graph); + GaussNewtonParams params, const NonlinearFactorGraph& graph) const { + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f7ef09773..52af4fee3 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -341,7 +341,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke Ordering order; if(constrainKeys) { - order = Ordering::colamdConstrained(variableIndex_, *constrainKeys); + order = Ordering::ColamdConstrained(variableIndex_, *constrainKeys); } else { @@ -351,11 +351,11 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke FastMap constraintGroups; BOOST_FOREACH(Key var, observedKeys) constraintGroups[var] = 1; - order = Ordering::colamdConstrained(variableIndex_, constraintGroups); + order = Ordering::ColamdConstrained(variableIndex_, constraintGroups); } else { - order = Ordering::colamd(variableIndex_); + order = Ordering::Colamd(variableIndex_); } } gttoc(ordering); @@ -481,7 +481,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::colamdConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 928b27167..59bb2d42b 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -254,6 +254,7 @@ void LevenbergMarquardtOptimizer::iterate() { bool systemSolvedSuccessfully; try { + // ============ Solve is where most computation happens !! ================= delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; } catch (const IndeterminantLinearSystemException& e) { @@ -281,7 +282,9 @@ void LevenbergMarquardtOptimizer::iterate() { if (linearizedCostChange >= 0) { // step is valid // update values gttic(retract); + // ============ This is where the solution is updated ==================== newValues = state_.values.retract(delta); + // ======================================================================= gttoc(retract); // compute new error @@ -361,12 +364,8 @@ void LevenbergMarquardtOptimizer::iterate() { /* ************************************************************************* */ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { - if (!params.ordering){ - if (params.orderingType == Ordering::METIS) - params.ordering = Ordering::metis(graph); - else - params.ordering = Ordering::colamd(graph); - } + if (!params.ordering) + params.ordering = Ordering::Create(params.orderingType, graph); return params; } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index fbc7190de..b81332f5e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -248,13 +248,13 @@ KeySet NonlinearFactorGraph::keys() const { /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMD() const { - return Ordering::colamd(*this); + return Ordering::Colamd(*this); } /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap& constraints) const { - return Ordering::colamdConstrained(*this, constraints); + return Ordering::ColamdConstrained(*this, constraints); } /* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 3c02f6dbd..93c38b324 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -691,6 +691,76 @@ TEST(SymbolicBayesTree, complicatedMarginal) } } + +/* ************************************************************************* */ +TEST(SymbolicBayesTree, COLAMDvsMETIS) { + + // create circular graph + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(4, 5); + sfg.push_factor(0, 5); + + // COLAMD + { + Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg); + EXPECT(assert_equal(Ordering(list_of(0)(5)(1)(4)(2)(3)), ordering)); + + // - P( 4 2 3) + // | - P( 1 | 2 4) + // | | - P( 5 | 1 4) + // | | | - P( 0 | 1 5) + SymbolicBayesTree expected; + expected.insertRoot( + MakeClique(list_of(4)(2)(3), 3, + list_of( + MakeClique(list_of(1)(2)(4), 1, + list_of( + MakeClique(list_of(5)(1)(4), 1, + list_of(MakeClique(list_of(0)(1)(5), 1)))))))); + + SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); + EXPECT(assert_equal(expected, actual)); + } + + // METIS + { + Ordering ordering = Ordering::Create(Ordering::METIS, sfg); +// Linux and Mac split differently when using mettis +#if !defined(__APPLE__) + EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering)); +#else + EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering)); +#endif + + // - P( 1 0 3) + // | - P( 4 | 0 3) + // | | - P( 5 | 0 4) + // | - P( 2 | 1 3) + SymbolicBayesTree expected; +#if !defined(__APPLE__) + expected.insertRoot( + MakeClique(list_of(2)(4)(1), 3, + list_of( + MakeClique(list_of(0)(1)(4), 1, + list_of(MakeClique(list_of(5)(0)(4), 1))))( + MakeClique(list_of(3)(2)(4), 1)))); +#else + expected.insertRoot( + MakeClique(list_of(1)(0)(3), 3, + list_of( + MakeClique(list_of(4)(0)(3), 1, + list_of(MakeClique(list_of(5)(0)(4), 1))))( + MakeClique(list_of(2)(1)(3), 1)))); +#endif + SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); + EXPECT(assert_equal(expected, actual)); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index aa13b1462..d74a0c302 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -205,7 +205,7 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { } // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::colamdConstrainedFirst(factors_, + ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); if (debug) { diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 1a1622134..c3e307010 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -362,9 +362,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& keysT // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 if(keysToMove && keysToMove->size() > 0) { - ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); + ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); }else{ - ordering_ = Ordering::colamd(factors_); + ordering_ = Ordering::Colamd(factors_); } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index e8a89cc17..29ee14aee 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() { variableIndex_ = VariableIndex(factors_); KeyVector separatorKeys = separatorValues_.keys(); - ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); + ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index b81a3113b..ee60f9714 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -79,14 +79,14 @@ TEST( NonlinearFactorGraph, GET_ORDERING) { Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 NonlinearFactorGraph nlfg = createNonlinearFactorGraph(); - Ordering actual = Ordering::colamd(nlfg); + Ordering actual = Ordering::Colamd(nlfg); EXPECT(assert_equal(expected,actual)); // Constrained ordering - put x2 at the end Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2); FastMap constraints; constraints[X(2)] = 1; - Ordering actualConstrained = Ordering::colamdConstrained(nlfg, constraints); + Ordering actualConstrained = Ordering::ColamdConstrained(nlfg, constraints); EXPECT(assert_equal(expectedConstrained, actualConstrained)); }