diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 429853ed0..7d9457b9e 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -18,6 +18,7 @@ // For an explanation of headers, see SFMExample.cpp #include +#include #include #include #include @@ -40,7 +41,7 @@ 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 = findExampleDataFile(string(argv[1])); + if (argc>1) filename = string(argv[1]); // Load the SfM data from file SfM_data mydata; @@ -79,76 +80,102 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ + /** ---------------------------------------------------**/ + + double t_COLAMD_ordering, t_METIS_ordering; //, t_NATURAL_ordering; + LevenbergMarquardtParams params_using_COLAMD, params_using_METIS, params_using_NATURAL; + try { + double tic_t = clock(); + params_using_METIS.setVerbosity("ERROR"); + params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); + t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; + + tic_t = clock(); + params_using_COLAMD.setVerbosity("ERROR"); + params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); + t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; + +// tic_t = clock(); +// params_using_NATURAL.setVerbosity("ERROR"); +// params_using_NATURAL.ordering = Ordering::Create(Ordering::NATURAL, graph); +// t_NATURAL_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; + + } 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; + } + + /* with METIS, optimize the graph and print the results */ + cout << "Optimize with METIS" << endl; + + Values result_METIS; + double t_METIS_solving; + try { + double tic_t = clock(); + LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_COLAMD); + result_METIS = lm_METIS.optimize(); + t_METIS_solving = (clock() - tic_t)/CLOCKS_PER_SEC; + } catch (exception& e) { + cout << e.what(); + } + /* With COLAMD, optimize the graph and print the results */ cout << "Optimize with COLAMD..." << endl; Values result_COLAMD; - double t_COLAMD_ordering, t_COLAMD_solving; + double t_COLAMD_solving; try { double tic_t = clock(); - - LevenbergMarquardtParams params_using_COLAMD; - params_using_COLAMD.setVerbosity("ERROR"); - params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); - - t_COLAMD_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; - - tic_t = clock(); - - LevenbergMarquardtOptimizer lm(graph, initial, params_using_COLAMD); - result_COLAMD = lm.optimize(); - + LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD); + result_COLAMD = lm_COLAMD.optimize(); t_COLAMD_solving = (clock() - tic_t)/CLOCKS_PER_SEC; - } catch (exception& e) { cout << e.what(); } + // disable optimizer with NATURAL since it doesn't converge on large problem + /* Use Natural ordering and solve both respectively */ + // cout << "Solving with natural ordering: " << endl; + + // Values result_NATURAL; + // double t_NATURAL_solving; + // try { + // double tic_t = clock(); + // LevenbergMarquardtOptimizer lm_NATURAL(graph, initial, params_using_NATURAL); + // result_NATURAL = lm_NATURAL.optimize(); + // t_NATURAL_solving = (clock() - tic_t)/CLOCKS_PER_SEC; + // } catch (exception& e) { + // cout << e.what(); + // } + cout << endl << endl; - // To see the error, check SFMExample_bal.cpp file - //cout << "final error: " << graph.error(result_COLAMD) << endl; - - /** ---------------------------------------------------**/ - /* with METIS, optimize the graph and print the results */ - - cout << "Optimize with METIS" << endl; - - Values results_METIS; - double t_METIS_ordering, t_METIS_solving; - try { - double tic_t = clock(); - - LevenbergMarquardtParams params_using_METIS; - params_using_METIS.setVerbosity("ERROR"); - params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); - - t_METIS_ordering = (clock() - tic_t)/CLOCKS_PER_SEC; - - tic_t = clock(); - - LevenbergMarquardtOptimizer lm(graph, initial, params_using_METIS); - results_METIS = lm.optimize(); - - t_METIS_solving = (clock() - tic_t)/CLOCKS_PER_SEC; - - } catch (exception& e) { - cout << e.what(); - } { // printing the result cout << "Time comparison by solving " << filename << " results:" << endl; - cout << boost::format("read %1% tracks on %2% cameras\n") \ + cout << boost::format("%1% point tracks and %2% cameras\n") \ % mydata.number_tracks() % mydata.number_cameras() \ << endl; cout << "COLAMD: " << endl; cout << "Ordering: " << t_COLAMD_ordering << "seconds" << endl; - cout << "Solving: " << t_COLAMD_solving << "seconds" << endl; + cout << "Solving: " << t_COLAMD_solving << "seconds" << endl; + cout << "final error: " << graph.error(result_COLAMD) << endl; cout << "METIS: " << endl; cout << "Ordering: " << t_METIS_ordering << "seconds" << endl; - cout << "Solving: " << t_METIS_solving << "seconds" << endl; + cout << "Solving: " << t_METIS_solving << "seconds" << endl; + cout << "final error: " << graph.error(result_METIS) << endl; + +// cout << "Natural: " << endl; +// cout << "Ordering: " << t_NATURAL_ordering << "seconds" << endl; +// cout << "Solving: " << t_NATURAL_solving << "seconds" << endl; +// cout << "final error: " << graph.error(result_NATURAL) << endl; } return 0; diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index e4cc2c55d..109fa1784 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -38,7 +38,7 @@ public: /// Type of ordering to use enum OrderingType { - COLAMD, METIS, CUSTOM + COLAMD, METIS, CUSTOM, NATURAL }; typedef Ordering This; ///< Typedef to this class @@ -163,7 +163,7 @@ public: /// Return a natural Ordering. Typically used by iterative solvers template - static Ordering Natural(const FactorGraph &fg) { + static Ordering natural(const FactorGraph &fg) { FastSet src = fg.keys(); std::vector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); @@ -196,6 +196,8 @@ public: 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."); diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index ab3ccde13..89486962b 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -86,7 +86,7 @@ KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) /****************************************************************************/ KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::Natural(fg)) { + : ordering_(Ordering::natural(fg)) { initialize(fg); } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index f5ee4ddfc..af4e9dd4f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -366,7 +366,7 @@ std::vector SubgraphBuilder::sample(const std::vector &weights, Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { const SubgraphBuilderParameters &p = parameters_; - const Ordering inverse_ordering = Ordering::Natural(gfg); + const Ordering inverse_ordering = Ordering::natural(gfg); const FastMap forward_ordering = inverse_ordering.invert(); const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;