12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added a new combine process for GaussianFactors that allocates only one matrix when combining, rather than using append factor to make a large number of smaller matrices. There is a flag to switch between these modes, which currently defaults to the older approach. Currently, there does not appear to be a performance improvement, however.

release/4.3a0
Alex Cunningham 2010-01-31 17:49:33 +00:00
parent 9f16d7b07f
commit 87cc573d34
7 changed files with 495 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
cpp/GaussianFactor.cpp
View File

@ -175,8 +175,7 @@ list<Symbol> 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<Matrix, SharedDiagonal> GaussianFactor::combineFactorsAndCreateMatrix(
const vector<GaussianFactor::shared_ptr>& 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<noiseModel::Unit>(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<noiseModel::Constrained>(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<int>, list<int>, list<double> >
GaussianFactor::sparse(const Dimensions& columnIndices) const {
@ -330,35 +397,20 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_
#include <boost/numeric/ublas/matrix_proxy.hpp>
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
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 m<n1, this factor cannot be eliminated
size_t maxRank = noiseModel->dim();
@ -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<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
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

20
cpp/GaussianFactor.h
View File

@ -265,6 +265,15 @@ public:
std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
eliminate(const Symbol& key) const;
/**
* Performs elimination given an augmented matrix
* @param
*/
static std::pair<boost::shared_ptr<GaussianConditional>, 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<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix(
const std::vector<GaussianFactor::shared_ptr>& factors,
const Ordering& order, const Dimensions& dimensions);
}; // GaussianFactor
/* ************************************************************************* */

64
cpp/GaussianFactorGraph.cpp
View File

@ -9,6 +9,7 @@
#include <boost/tuple/tuple.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/assign/std/list.hpp> // for operator += in Ordering
#include <colamd/colamd.h>
@ -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<Symbol> GaussianFactorGraph::find_separator(const Symbol& key) const
/* ************************************************************************* */
GaussianConditional::shared_ptr
GaussianFactorGraph::eliminateOne(const Symbol& key) {
return gtsam::eliminateOne<GaussianFactor,GaussianConditional>(*this, key);
GaussianFactorGraph::eliminateOne(const Symbol& key, bool old) {
#ifdef USE_FAST_ELIMINATE
return eliminateOneMatrixJoin(key);
#else
if (old)
return gtsam::eliminateOne<GaussianFactor,GaussianConditional>(*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<GaussianFactor::shared_ptr> factors = findAndRemoveFactors(key);
set<Symbol> 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);

19
cpp/GaussianFactorGraph.h
View File

@ -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

53
cpp/testGaussianFactor.cpp
View File

@ -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<Symbol> 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<GaussianFactor::shared_ptr> 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 )

69
cpp/testGaussianFactorGraph.cpp
View File

@ -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 )
{

198
cpp/timeGaussianFactorGraph.cpp
View File

@ -7,6 +7,8 @@
#define GTSAM_MAGIC_KEY
#include <time.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator += in Ordering
#include <CppUnitLite/TestHarness.h>
#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; i<reps; ++i) {
// setup
GaussianFactorGraph fg(fgBase);
// combine some factors
GaussianFactor::shared_ptr joint_factor = removeAndCombineFactors(fg,key);
// create an internal ordering to render Ab
Ordering render;
render += key;
BOOST_FOREACH(const Symbol& k, joint_factor->keys())
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<reps; ++i) {
// setup
GaussianFactorGraph fg(fgBase);
Ordering render; render += key; // start with variable to eliminate
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors(key);
set<Symbol> 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;