diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index f37c555b1..1ed569239 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -175,8 +175,7 @@ list GaussianFactor::keys() const { /* ************************************************************************* */ Dimensions GaussianFactor::dimensions() const { Dimensions result; - FOREACH_PAIR(j,Aj,As_) - result.insert(make_pair(*j,Aj->size2())); + FOREACH_PAIR(j,Aj,As_) result.insert(make_pair(*j,Aj->size2())); return result; } @@ -264,6 +263,74 @@ Matrix GaussianFactor::matrix_augmented(const Ordering& ordering, bool weight) c return Ab; } +/* ************************************************************************* */ +std::pair GaussianFactor::combineFactorsAndCreateMatrix( + const vector& factors, + const Ordering& order, const Dimensions& dimensions) { + // find the size of Ab + size_t m = 0, n = 1; + + // number of rows + BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { + m += factor->numberOfRows(); + } + + // find the number of columns + BOOST_FOREACH(const Symbol& key, order) { + n += dimensions.at(key); + } + + // Allocate the new matrix + Matrix Ab = zeros(m,n); + + // Allocate a sigmas vector to make into a full noisemodel + Vector sigmas = ones(m); + + // copy data over + size_t cur_m = 0; + bool constrained = false; + bool unit = true; + BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { + // loop through ordering + size_t cur_n = 0; + BOOST_FOREACH(const Symbol& key, order) { + // copy over matrix if it exists + if (factor->involves(key)) { + insertSub(Ab, factor->get_A(key), cur_m, cur_n); + } + // move onto next element + cur_n += dimensions.at(key); + } + // copy over the RHS + insertColumn(Ab, factor->get_b(), cur_m, n-1); + + // check if the model is unit already + if (!boost::shared_dynamic_cast(factor->get_model())) { + unit = false; + const Vector& subsigmas = factor->get_model()->sigmas(); + subInsert(sigmas, subsigmas, cur_m); + + // check for constraint + if (boost::shared_dynamic_cast(factor->get_model())) + constrained = true; + } + + // move to next row + cur_m += factor->numberOfRows(); + } + + // combine the noisemodels + SharedDiagonal model; + if (unit) { + model = noiseModel::Unit::Create(m); + } else if (constrained) { + model = noiseModel::Constrained::MixedSigmas(sigmas); + } else { + model = noiseModel::Diagonal::Sigmas(sigmas); + } + return make_pair(Ab, model); +} + /* ************************************************************************* */ boost::tuple, list, list > GaussianFactor::sparse(const Dimensions& columnIndices) const { @@ -330,35 +397,20 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_ #include pair -GaussianFactor::eliminate(const Symbol& key) const -{ +GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model, + const Ordering& ordering, + const Dimensions& dimensions) { bool verbose = false; - // if this factor does not involve key, we exit with empty CG and LF - const_iterator it = As_.find(key); - if (it==As_.end()) { - // Conditional Gaussian is just a parent-less node with P(x)=1 - GaussianFactor::shared_ptr lf(new GaussianFactor); - GaussianConditional::shared_ptr cg(new GaussianConditional(key)); - return make_pair(cg,lf); - } - - // create an internal ordering that eliminates key first - Ordering ordering; - ordering += key; - BOOST_FOREACH(const Symbol& k, keys()) - if (k != key) ordering += k; - - // extract [A b] from the combined linear factor (ensure that x is leading) - Matrix Ab = matrix_augmented(ordering,false); + Symbol key = ordering.front(); // Use in-place QR on dense Ab appropriate to NoiseModel - if (verbose) model_->print("Before QR"); - SharedDiagonal noiseModel = model_->QR(Ab); - if (verbose) model_->print("After QR"); + if (verbose) model->print("Before QR"); + SharedDiagonal noiseModel = model->QR(Ab); + if (verbose) model->print("After QR"); // get dimensions of the eliminated variable // TODO: this is another map find that should be avoided ! - size_t n1 = getDim(key), n = Ab.size2() - 1; + size_t n1 = dimensions.at(key), n = Ab.size2() - 1; // if mdim(); @@ -382,9 +434,9 @@ GaussianFactor::eliminate(const Symbol& key) const // extract the block matrices for parents in both CG and LF GaussianFactor::shared_ptr factor(new GaussianFactor); size_t j = n1; - BOOST_FOREACH(Symbol& cur_key, ordering) + BOOST_FOREACH(const Symbol& cur_key, ordering) if (cur_key!=key) { - size_t dim = getDim(cur_key); // TODO avoid find ! + size_t dim = dimensions.at(cur_key); // TODO avoid find ! conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim)); factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly j+=dim; @@ -403,6 +455,32 @@ GaussianFactor::eliminate(const Symbol& key) const return make_pair(conditional, factor); } +pair +GaussianFactor::eliminate(const Symbol& key) const +{ + bool verbose = false; + // if this factor does not involve key, we exit with empty CG and LF + const_iterator it = As_.find(key); + if (it==As_.end()) { + // Conditional Gaussian is just a parent-less node with P(x)=1 + GaussianFactor::shared_ptr lf(new GaussianFactor); + GaussianConditional::shared_ptr cg(new GaussianConditional(key)); + return make_pair(cg,lf); + } + + // create an internal ordering that eliminates key first + Ordering ordering; + ordering += key; + BOOST_FOREACH(const Symbol& k, keys()) + if (k != key) ordering += k; + + // extract [A b] from the combined linear factor (ensure that x is leading) + Matrix Ab = matrix_augmented(ordering,false); + + // TODO: this is where to split + return eliminateMatrix(Ab, model_, ordering, dimensions()); +} + /* ************************************************************************* */ // Creates a factor on step-size, given initial estimate and direction d, e.g. // Factor |A1*x+A2*y-b|/sigma -> |A1*(x0+alpha*dx)+A2*(y0+alpha*dy)-b|/sigma diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 2714ce1d4..f0ffe03da 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -265,6 +265,15 @@ public: std::pair, shared_ptr> eliminate(const Symbol& key) const; + /** + * Performs elimination given an augmented matrix + * @param + */ + static std::pair, shared_ptr> + eliminateMatrix(Matrix& Ab, SharedDiagonal model, + const Ordering& ordering, + const Dimensions& dimensions); + /** * Take the factor f, and append to current matrices. Not very general. * @param f linear factor graph @@ -273,6 +282,17 @@ public: */ void append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos); + /** + * Returns the augmented matrix version of a set of factors + * with the corresponding noiseModel + * @param factors is the set of factors to combine + * @param ordering of variables needed for matrix column order + * @return the augmented matrix and a noise model + */ + static std::pair combineFactorsAndCreateMatrix( + const std::vector& factors, + const Ordering& order, const Dimensions& dimensions); + }; // GaussianFactor /* ************************************************************************* */ diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 61fa680a8..4947deb05 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -9,6 +9,7 @@ #include #include #include +#include // for operator += in Ordering #include @@ -20,6 +21,10 @@ using namespace std; using namespace gtsam; +using namespace boost::assign; + +//#define USE_FAST_ELIMINATE + // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) @@ -114,24 +119,71 @@ set GaussianFactorGraph::find_separator(const Symbol& key) const /* ************************************************************************* */ GaussianConditional::shared_ptr -GaussianFactorGraph::eliminateOne(const Symbol& key) { - return gtsam::eliminateOne(*this, key); +GaussianFactorGraph::eliminateOne(const Symbol& key, bool old) { +#ifdef USE_FAST_ELIMINATE + return eliminateOneMatrixJoin(key); +#else + if (old) + return gtsam::eliminateOne(*this, key); + else + return eliminateOneMatrixJoin(key); +#endif +} + +/* ************************************************************************* */ +GaussianConditional::shared_ptr +GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) { + // get a vector of all of the factors in the separator as well as an ordering + vector factors = findAndRemoveFactors(key); + + set separator; + Dimensions dimensions; + BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { + Dimensions factor_dim = factor->dimensions(); + dimensions.insert(factor_dim.begin(), factor_dim.end()); + BOOST_FOREACH(const Symbol& k, factor->keys()) { + if (!k.equals(key)) { + separator.insert(k); + } + } + } + + // add the keys to the rendering + Ordering render; render += key; + BOOST_FOREACH(const Symbol& k, separator) + if (k != key) render += k; + + // combine the factors to get a noisemodel and a combined matrix + Matrix Ab; SharedDiagonal model; + + boost::tie(Ab, model) = GaussianFactor::combineFactorsAndCreateMatrix(factors,render,dimensions); + + // eliminate that joint factor + GaussianFactor::shared_ptr factor; + GaussianConditional::shared_ptr conditional; + boost::tie(conditional, factor) = GaussianFactor::eliminateMatrix(Ab, model, render, dimensions); + + // add new factor on separator back into the graph + if (!factor->empty()) push_back(factor); + + // return the conditional Gaussian + return conditional; } /* ************************************************************************* */ GaussianBayesNet -GaussianFactorGraph::eliminate(const Ordering& ordering) +GaussianFactorGraph::eliminate(const Ordering& ordering, bool old) { GaussianBayesNet chordalBayesNet; // empty BOOST_FOREACH(const Symbol& key, ordering) { - GaussianConditional::shared_ptr cg = eliminateOne(key); + GaussianConditional::shared_ptr cg = eliminateOne(key, old); chordalBayesNet.push_back(cg); } return chordalBayesNet; } /* ************************************************************************* */ -VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering) +VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old) { bool verbose = false; if (verbose) @@ -139,7 +191,7 @@ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering) factor->get_model()->print("Starting model"); // eliminate all nodes in the given ordering -> chordal Bayes net - GaussianBayesNet chordalBayesNet = eliminate(ordering); + GaussianBayesNet chordalBayesNet = eliminate(ordering, old); // calculate new configuration (using backsubstitution) return ::optimize(chordalBayesNet); diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index e1c5eface..8583f0163 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -119,21 +119,34 @@ namespace gtsam { * Eliminate a single node yielding a conditional Gaussian * Eliminates the factors from the factor graph through findAndRemoveFactors * and adds a new factor on the separator to the factor graph + * @param key is the key to eliminate + * @param enableJoinFactor uses the older joint factor combine process when true, + * and when false uses the newer single matrix combine */ - GaussianConditional::shared_ptr eliminateOne(const Symbol& key); + GaussianConditional::shared_ptr eliminateOne(const Symbol& key, bool enableJoinFactor = true); + + /** + * Peforms a supposedly-faster (fewer matrix copy) version of elimination + * CURRENTLY IN TESTING + */ + inline GaussianConditional::shared_ptr eliminateOneMatrixJoin(const Symbol& key); /** * eliminate factor graph in place(!) in the given order, yielding * a chordal Bayes net. Allows for passing an incomplete ordering * that does not completely eliminate the graph + * @param enableJoinFactor uses the older joint factor combine process when true, + * and when false uses the newer single matrix combine */ - GaussianBayesNet eliminate(const Ordering& ordering); + GaussianBayesNet eliminate(const Ordering& ordering, bool enableJoinFactor = true); /** * optimize a linear factor graph * @param ordering fg in order + * @param enableJoinFactor uses the older joint factor combine process when true, + * and when false uses the newer single matrix combine */ - VectorConfig optimize(const Ordering& ordering); + VectorConfig optimize(const Ordering& ordering, bool enableJoinFactor = true); /** * shared pointer versions for MATLAB diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index b596b6d77..a243023ba 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -628,6 +628,29 @@ TEST( GaussianFactor, size ) CHECK(factor3->size() == 2); } +/* ************************************************************************* */ +TEST( GaussianFactor, tally_separator ) +{ + GaussianFactor f("x1", eye(2), "x2", eye(2), "l1", eye(2), ones(2), sigma0_1); + + std::set act1, act2, act3; + f.tally_separator("x1", act1); + f.tally_separator("x2", act2); + f.tally_separator("l1", act3); + + CHECK(act1.size() == 2); + CHECK(act1.count("x2") == 1); + CHECK(act1.count("l1") == 1); + + CHECK(act2.size() == 2); + CHECK(act2.count("x1") == 1); + CHECK(act2.count("l1") == 1); + + CHECK(act3.size() == 2); + CHECK(act3.count("x1") == 1); + CHECK(act3.count("x2") == 1); +} + /* ************************************************************************* */ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional ) { @@ -728,6 +751,36 @@ TEST ( GaussianFactor, constraint_eliminate2 ) GaussianConditional expectedCG("x", d, R, "y", S, zero(2)); CHECK(assert_equal(expectedCG, *actualCG, 1e-4)); } +/* ************************************************************************* */ +TEST ( GaussianFactor, combine_matrix ) { + // create a small linear factor graph + GaussianFactorGraph fg = createGaussianFactorGraph(); + Dimensions dimensions = fg.dimensions(); + + // get two factors from it and insert the factors into a vector + vector lfg; + lfg.push_back(fg[4 - 1]); + lfg.push_back(fg[2 - 1]); + + // combine in a factor + Matrix Ab; SharedDiagonal noise; + Ordering order; order += "x2", "l1", "x1"; + boost::tie(Ab, noise) = GaussianFactor::combineFactorsAndCreateMatrix(lfg, order, dimensions); + + // the expected augmented matrix + Matrix expAb = Matrix_(4, 7, + -5., 0., 5., 0., 0., 0.,-1.0, + +0., -5., 0., 5., 0., 0., 1.5, + 10., 0., 0., 0.,-10., 0., 2.0, + +0., 10., 0., 0., 0.,-10.,-1.0); + + // expected noise model + SharedDiagonal expModel = noiseModel::Unit::Create(4); + + CHECK(assert_equal(expAb, Ab)); + CHECK(assert_equal(*expModel, *noise)); +} + ///* ************************************************************************* * //TEST ( GaussianFactor, constraint_eliminate3 ) diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 35db07d6a..75833a82b 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -223,6 +223,50 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) CHECK(assert_equal(expected,*actual,tol)); } +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x1_fast ) +{ + GaussianFactorGraph fg = createGaussianFactorGraph(); + GaussianConditional::shared_ptr actual = fg.eliminateOne("x1", false); + + // create expected Conditional Gaussian + Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); + GaussianConditional expected("x1",15*d,R11,"l1",S12,"x2",S13,sigma); + + CHECK(assert_equal(expected,*actual,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x2_fast ) +{ + GaussianFactorGraph fg = createGaussianFactorGraph(); + GaussianConditional::shared_ptr actual = fg.eliminateOne("x2", false); + + // create expected Conditional Gaussian + double sig = 0.0894427; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); + GaussianConditional expected("x2",d,R11,"l1",S12,"x1",S13,sigma); + + CHECK(assert_equal(expected,*actual,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_l1_fast ) +{ + GaussianFactorGraph fg = createGaussianFactorGraph(); + GaussianConditional::shared_ptr actual = fg.eliminateOne("l1", false); + + // create expected Conditional Gaussian + double sig = sqrt(2)/10.; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); + GaussianConditional expected("l1",d,R11,"x1",S12,"x2",S13,sigma); + + CHECK(assert_equal(expected,*actual,tol)); +} + /* ************************************************************************* */ TEST( GaussianFactorGraph, eliminateAll ) { @@ -248,6 +292,31 @@ TEST( GaussianFactorGraph, eliminateAll ) CHECK(assert_equal(expected,actual,tol)); } +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateAll_fast ) +{ + // create expected Chordal bayes Net + Matrix I = eye(2); + + Vector d1 = Vector_(2, -0.1,-0.1); + GaussianBayesNet expected = simpleGaussian("x1",d1,0.1); + + double sig1 = 0.149071; + Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); + push_front(expected,"l1",d2, I/sig1,"x1", (-1)*I/sig1,sigma2); + + double sig2 = 0.0894427; + Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); + push_front(expected,"x2",d3, I/sig2,"l1", (-0.2)*I/sig2, "x1", (-0.8)*I/sig2, sigma3); + + // Check one ordering + GaussianFactorGraph fg1 = createGaussianFactorGraph(); + Ordering ordering; + ordering += "x2","l1","x1"; + GaussianBayesNet actual = fg1.eliminate(ordering, false); + CHECK(assert_equal(expected,actual,tol)); +} + /* ************************************************************************* */ TEST( GaussianFactorGraph, add_priors ) { diff --git a/cpp/timeGaussianFactorGraph.cpp b/cpp/timeGaussianFactorGraph.cpp index d03208441..8ac11fc11 100644 --- a/cpp/timeGaussianFactorGraph.cpp +++ b/cpp/timeGaussianFactorGraph.cpp @@ -7,6 +7,8 @@ #define GTSAM_MAGIC_KEY #include +#include +#include // for operator += in Ordering #include #include "smallExample.h" #include "Ordering.h" @@ -14,6 +16,7 @@ using namespace std; using namespace gtsam; using namespace example; +using namespace boost::assign; /* ************************************************************************* */ // Create a Kalman smoother for t=1:T and optimize @@ -30,18 +33,113 @@ double timeKalmanSmoother(int T) { /* ************************************************************************* */ // Create a planar factor graph and optimize -double timePlanarSmoother(int N) { +double timePlanarSmoother(int N, bool old = true) { GaussianFactorGraph fg; VectorConfig config; boost::tie(fg,config) = planarGraph(N); Ordering ordering = fg.getOrdering(); clock_t start = clock(); - fg.optimize(ordering); + fg.optimize(ordering, old); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; } +/* ************************************************************************* */ +// Create a planar factor graph and eliminate +double timePlanarSmootherEliminate(int N, bool old = true) { + GaussianFactorGraph fg; + VectorConfig config; + boost::tie(fg,config) = planarGraph(N); + Ordering ordering = fg.getOrdering(); + clock_t start = clock(); + fg.eliminate(ordering, old); + clock_t end = clock (); + double dif = (double)(end - start) / CLOCKS_PER_SEC; + return dif; +} + +/* ************************************************************************* */ +// Create a planar factor graph and join factors until matrix formation +// This variation uses the original join factors approach +double timePlanarSmootherJoinAug(int N, size_t reps) { + GaussianFactorGraph fgBase; + VectorConfig config; + boost::tie(fgBase,config) = planarGraph(N); + Ordering ordering = fgBase.getOrdering(); + Symbol key = ordering.front(); + + clock_t start = clock(); + + for (size_t i = 0; ikeys()) + if (k != key) render += k; + + Matrix Ab = joint_factor->matrix_augmented(render,false); + } + + clock_t end = clock (); + double dif = (double)(end - start) / CLOCKS_PER_SEC; + return dif; +} + +/* ************************************************************************* */ +// Create a planar factor graph and join factors until matrix formation +// This variation uses the single-allocate version to create the matrix +double timePlanarSmootherCombined(int N, size_t reps) { + GaussianFactorGraph fgBase; + VectorConfig config; + boost::tie(fgBase,config) = planarGraph(N); + Ordering ordering = fgBase.getOrdering(); + Symbol key = ordering.front(); + + clock_t start = clock(); + + for (size_t i = 0; i factors = fg.findAndRemoveFactors(key); + + set separator; + Dimensions dimensions; + BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { + Dimensions factor_dim = factor->dimensions(); + dimensions.insert(factor_dim.begin(), factor_dim.end()); + BOOST_FOREACH(const Symbol& k, factor->keys()) { + if (!k.equals(key)) { + separator.insert(k); + } + } + } + + // add the keys to the rendering + BOOST_FOREACH(const Symbol& k, separator) + if (k != key) render += k; + + // combine the factors to get a noisemodel and a combined matrix + Matrix Ab; SharedDiagonal model; + + boost::tie(Ab, model) = GaussianFactor::combineFactorsAndCreateMatrix(factors,render,dimensions); + + } + + clock_t end = clock (); + double dif = (double)(end - start) / CLOCKS_PER_SEC; + return dif; +} + + /* ************************************************************************* */ TEST(timeGaussianFactorGraph, linearTime) { @@ -59,33 +157,85 @@ TEST(timeGaussianFactorGraph, linearTime) DOUBLES_EQUAL(2*time1,time2,0.2); } + +// Timing with N = 30 +// 1741: 8.12, 8.12, 8.12, 8.14, 8.16 +// 1742: 5.97, 5.97, 5.97, 5.99, 6.02 +// 1746: 5.96, 5.96, 5.97, 6.00, 6.04 +// 1748: 5.91, 5.92, 5.93, 5.95, 5.96 +// 1839: 0.206956 0.206939 0.206213 0.206092 0.206780 // colamd !!!! + +// Alex's Machine +// Initial: +// 1907 (N = 30) : 0.14 +// (N = 100) : 16.36 +// Improved (int->size_t) +// (N = 100) : 15.39 +// Using GSL/BLAS for updateAb : 12.87 +// Using NoiseQR : 16.33 +// Using correct system : 10.00 + +// Switch to 100*100 grid = 10K poses +// 1879: 15.6498 15.3851 15.5279 + +int size = 100; + /* ************************************************************************* */ -TEST(timeGaussianFactorGraph, planar) +TEST(timeGaussianFactorGraph, planar_old) { - // Timing with N = 30 - // 1741: 8.12, 8.12, 8.12, 8.14, 8.16 - // 1742: 5.97, 5.97, 5.97, 5.99, 6.02 - // 1746: 5.96, 5.96, 5.97, 6.00, 6.04 - // 1748: 5.91, 5.92, 5.93, 5.95, 5.96 - // 1839: 0.206956 0.206939 0.206213 0.206092 0.206780 // colamd !!!! - - // Alex's Machine - // Initial: - // 1907 (N = 30) : 0.14 - // (N = 100) : 16.36 - // Improved (int->size_t) - // (N = 100) : 15.39 - // Using GSL/BLAS for updateAb : 12.87 - // Using NoiseQR : 16.33 - // Using correct system : 10.00 - - // Switch to 100*100 grid = 10K poses - // 1879: 15.6498 15.3851 15.5279 - int N = 100; - double time = timePlanarSmoother(N); cout << "timeGaussianFactorGraph : " << time << endl; + cout << "Timing planar - original version" << endl; + double time = timePlanarSmoother(size); + cout << "timeGaussianFactorGraph : " << time << endl; //DOUBLES_EQUAL(5.97,time,0.1); } +/* ************************************************************************* */ +TEST(timeGaussianFactorGraph, planar_new) +{ + cout << "Timing planar - new version" << endl; + double time = timePlanarSmoother(size, false); + cout << "timeGaussianFactorGraph : " << time << endl; + //DOUBLES_EQUAL(5.97,time,0.1); +} + +/* ************************************************************************* */ +TEST(timeGaussianFactorGraph, planar_eliminate_old) +{ + cout << "Timing planar Eliminate - original version" << endl; + double time = timePlanarSmootherEliminate(size); + cout << "timeGaussianFactorGraph : " << time << endl; + //DOUBLES_EQUAL(5.97,time,0.1); +} + +/* ************************************************************************* */ +TEST(timeGaussianFactorGraph, planar_eliminate_new) +{ + cout << "Timing planar Eliminate - new version" << endl; + double time = timePlanarSmootherEliminate(size, false); + cout << "timeGaussianFactorGraph : " << time << endl; + //DOUBLES_EQUAL(5.97,time,0.1); +} + +size_t reps = 1000; +/* ************************************************************************* */ +TEST(timeGaussianFactorGraph, planar_join_old) +{ + cout << "Timing planar join - old" << endl; + double time = timePlanarSmootherJoinAug(size, reps); + cout << "timePlanarSmootherJoinAug " << size << " : " << time << endl; + //DOUBLES_EQUAL(5.97,time,0.1); +} + +/* ************************************************************************* */ +TEST(timeGaussianFactorGraph, planar_join_new) +{ + cout << "Timing planar join - new" << endl; + double time = timePlanarSmootherCombined(size, reps); + cout << "timePlanarSmootherCombined " << size << " : " << time << endl; + //DOUBLES_EQUAL(5.97,time,0.1); +} + + /* ************************************************************************* */ int main() { TestResult tr;