137 lines
3.8 KiB
C++
137 lines
3.8 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 timeGaussianFactor.cpp
|
|
* @brief time JacobianFactor.eliminate
|
|
* @author Alireza Fathi
|
|
*/
|
|
|
|
#include <time.h>
|
|
|
|
/*STL/C++*/
|
|
#include <iostream>
|
|
using namespace std;
|
|
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include <boost/assign/list_of.hpp>
|
|
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <gtsam/linear/JacobianFactor.h>
|
|
#include <gtsam/linear/JacobianFactor.h>
|
|
#include <gtsam/linear/GaussianConditional.h>
|
|
#include <gtsam/linear/NoiseModel.h>
|
|
|
|
using namespace gtsam;
|
|
using namespace boost::assign;
|
|
|
|
static const Key _x1_=1, _x2_=2, _l1_=3;
|
|
|
|
/*
|
|
* Alex's Machine
|
|
* Results for Eliminate:
|
|
* Initial (1891): 17.91 sec, 55834.7 calls/sec
|
|
* NoiseQR : 11.69 sec
|
|
*
|
|
* Results for matrix_augmented:
|
|
* Initial (1891) : 0.85 sec, 1.17647e+06 calls/sec
|
|
* int->size_t Version : 8.45 sec (for n1 reps) with memcpy version of collect()
|
|
* w/ original collect(): 8.73 sec (for n1 reps)
|
|
* b memcpy Version : 8.64 sec (for n1 reps) with original version of collect()
|
|
* w/ memcpy collect() : 8.40 sec (for n1 reps)
|
|
* Rev 2100 : 8.15 sec
|
|
*/
|
|
|
|
int main()
|
|
{
|
|
// create a linear factor
|
|
Matrix Ax2 = (Matrix(8, 2) <<
|
|
// x2
|
|
-5., 0.,
|
|
+0.,-5.,
|
|
10., 0.,
|
|
+0.,10.,
|
|
-5., 0.,
|
|
+0.,-5.,
|
|
10., 0.,
|
|
+0.,10.
|
|
).finished();
|
|
|
|
Matrix Al1 = (Matrix(8, 10) <<
|
|
// l1
|
|
5., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0., 5.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
5., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0., 5.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0., 0.,1.,2.,3.,4.,5.,6.,7.,8.
|
|
).finished();
|
|
|
|
Matrix Ax1 = (Matrix(8, 2) <<
|
|
// x1
|
|
0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
-10., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0.00,-10.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
-10., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
|
|
0.00,-10.,1.,2.,3.,4.,5.,6.,7.,8.
|
|
).finished();
|
|
|
|
// and a RHS
|
|
Vector b2(8);
|
|
b2(0) = -1;
|
|
b2(1) = 1.5;
|
|
b2(2) = 2;
|
|
b2(3) = -1;
|
|
b2(4) = -1;
|
|
b2(5) = 1.5;
|
|
b2(6) = 2;
|
|
b2(7) = -1;
|
|
|
|
// time eliminate
|
|
JacobianFactor combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(8,1));
|
|
long timeLog = clock();
|
|
int n = 1000000;
|
|
GaussianConditional::shared_ptr conditional;
|
|
JacobianFactor::shared_ptr factor;
|
|
|
|
for(int i = 0; i < n; i++)
|
|
boost::tie(conditional, factor) =
|
|
JacobianFactor(combined).eliminate(Ordering(boost::assign::list_of(_x2_)));
|
|
|
|
long timeLog2 = clock();
|
|
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
|
|
cout << "Single Eliminate Timing:" << endl;
|
|
cout << seconds << " seconds" << endl;
|
|
cout << ((double)n/seconds) << " calls/second" << endl;
|
|
|
|
// time matrix_augmented
|
|
// Ordering ordering;
|
|
// ordering += _x2_, _l1_, _x1_;
|
|
// size_t n1 = 10000000;
|
|
// timeLog = clock();
|
|
//
|
|
// for(size_t i = 0; i < n1; i++)
|
|
// Matrix Ab = combined.matrix_augmented(ordering, true);
|
|
//
|
|
// timeLog2 = clock();
|
|
// seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
|
|
// cout << "matrix_augmented Timing:" << endl;
|
|
// cout << seconds << " seconds" << endl;
|
|
// cout << ((double)n/seconds) << " calls/second" << endl;
|
|
|
|
return 0;
|
|
}
|