diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 145b8d5ca..1b84364c0 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -86,7 +86,10 @@ int main(int argc, char** argv) { initial.print("\nInitial Estimate:\n"); // print // optimize using Levenberg-Marquardt optimization - Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + LevenbergMarquardtParams params; + params.orderingType = Ordering::Type::METIS_; + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); result.print("Final Result:\n"); // Calculate and print marginal covariances for all variables diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 0fe65e4c0..e14ba329b 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::Type::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); } } diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 6da83b8bc..08a0566a2 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -29,6 +29,7 @@ namespace gtsam { { std::map > adjMap; std::map >::iterator adjMapIt; + std::set values; /* ********** Convert to CSR format ********** */ // Assuming that vertex numbering starts from 0 (C style), @@ -37,13 +38,19 @@ namespace gtsam { // index xadj[i + 1](i.e., adjncy[xadj[i]] through // and including adjncy[xadj[i + 1] - 1]). for (size_t i = 0; i < factors.size(); i++){ - if (factors[i]) - BOOST_FOREACH(const Key& k1, *factors[i]) - BOOST_FOREACH(const Key& k2, *factors[i]) - if (k1 != k2) - adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end + if (factors[i]){ + BOOST_FOREACH(const Key& k1, *factors[i]){ + BOOST_FOREACH(const Key& k2, *factors[i]){ + if (k1 != k2) + adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end + } + values.insert(values.end(), k1); // Keep a track of all unique values + } + } } + // Number of values referenced in this factorgraph + nValues_ = values.size(); xadj_.push_back(0);// Always set the first index to zero for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index bcc9fc23f..7bc595aba 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -71,6 +71,7 @@ public: std::vector xadj() const { return xadj_; } std::vector adj() const { return adj_; } + size_t nValues() const { return nValues_; } /// @} }; diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index fbda41ac0..2618d8f50 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -202,30 +202,30 @@ namespace gtsam { /* ************************************************************************* */ Ordering Ordering::METIS(const MetisIndex& met) { - gttic(Ordering_METIS); + gttic(Ordering_METIS); - vector xadj = met.xadj(); - vector adj = met.adj(); + vector xadj = met.xadj(); + vector adj = met.adj(); - vector perm, iperm; + vector perm, iperm; - idx_t size = xadj.size() - 1; + idx_t size = met.nValues(); for (idx_t i = 0; i < size; i++) { perm.push_back(0); iperm.push_back(0); } - int outputError; + int outputError; - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); - Ordering result; + 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; - } + 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; ++j) diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index b769551bf..424eb7c19 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -116,17 +116,52 @@ TEST(Ordering, csr_format) { EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); - EXPECT( adjExpected == mi.adj()); + EXPECT(adjExpected == mi.adj()); } +/* ************************************************************************* */ + +TEST(Ordering, csr_format_2) { + SymbolicFactorGraph sfg; + + sfg.push_factor(0); + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(4, 1); + + MetisIndex mi(sfg); + + vector xadjExpected { 0, 1, 4, 6, 8, 10 }; + vector 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()); + + //Ordering metis = Ordering::METIS(sfg); + +} + /* ************************************************************************* */ TEST(Ordering, metis) { - SymbolicFactorGraph sfg; + SymbolicFactorGraph sfg; - sfg.push_factor(0, 1); + sfg.push_factor(0); + sfg.push_factor(0, 1); sfg.push_factor(1, 2); - Ordering metis = Ordering::METIS(sfg); + MetisIndex mi(sfg); + + vector xadjExpected{ 0, 1, 3, 4 }; + vector adjExpected { 1, 0, 2, 1 }; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); + + Ordering metis = Ordering::METIS(sfg); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 002f8b237..5fca680a2 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -341,7 +341,10 @@ void LevenbergMarquardtOptimizer::iterate() { LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { if (!params.ordering) - params.ordering = Ordering::COLAMD(graph); + if (params.orderingType = Ordering::Type::METIS_) + params.ordering = Ordering::METIS(graph); + else + params.ordering = Ordering::COLAMD(graph); return params; } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 04877c72e..d7ead9ca3 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -154,7 +154,7 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; - this->orderingType = Ordering::Type::CUSTOM_; + this->orderingType = Ordering::Type::CUSTOM_; } std::string getOrderingType() const {