158 lines
3.9 KiB
C++
158 lines
3.9 KiB
C++
/**
|
|
* @file timeSchurFactors.cpp
|
|
* @brief Time various Schur-complement Jacobian factors
|
|
* @author Frank Dellaert
|
|
* @date Oct 27, 2013
|
|
*/
|
|
|
|
#include "DummyFactor.h"
|
|
#include <gtsam/base/timing.h>
|
|
|
|
#include <gtsam/slam/JacobianFactorQ.h>
|
|
#include "gtsam/slam/JacobianFactorQR.h"
|
|
#include <gtsam/slam/RegularImplicitSchurFactor.h>
|
|
#include <gtsam/geometry/Cal3Bundler.h>
|
|
#include <gtsam/geometry/PinholePose.h>
|
|
|
|
#include <boost/assign/list_of.hpp>
|
|
#include <boost/assign/std/vector.hpp>
|
|
#include <fstream>
|
|
|
|
using namespace std;
|
|
using namespace boost::assign;
|
|
using namespace gtsam;
|
|
|
|
#define SLOW
|
|
#define RAW
|
|
#define HESSIAN
|
|
#define NUM_ITERATIONS 1000
|
|
|
|
// Create CSV file for results
|
|
ofstream os("timeSchurFactors.csv");
|
|
|
|
/*************************************************************************************/
|
|
template<typename CAMERA>
|
|
void timeAll(size_t m, size_t N) {
|
|
|
|
cout << m << endl;
|
|
|
|
// create F
|
|
static const int D = CAMERA::dimension;
|
|
typedef Eigen::Matrix<double, 2, D> Matrix2D;
|
|
FastVector<Key> keys;
|
|
vector <Matrix2D> Fblocks;
|
|
for (size_t i = 0; i < m; i++) {
|
|
keys.push_back(i);
|
|
Fblocks.push_back((i + 1) * Matrix::Ones(2, D));
|
|
}
|
|
|
|
// create E
|
|
Matrix E(2 * m, 3);
|
|
for (size_t i = 0; i < m; i++)
|
|
E.block < 2, 3 > (2 * i, 0) = Matrix::Ones(2, 3);
|
|
|
|
// Calculate point covariance
|
|
Matrix P = (E.transpose() * E).inverse();
|
|
|
|
// RHS and sigmas
|
|
const Vector b = gtsam::repeat(2 * m, 1);
|
|
const SharedDiagonal model;
|
|
|
|
// parameters for multiplyHessianAdd
|
|
double alpha = 0.5;
|
|
VectorValues xvalues, yvalues;
|
|
for (size_t i = 0; i < m; i++)
|
|
xvalues.insert(i, gtsam::repeat(D, 2));
|
|
|
|
// Implicit
|
|
RegularImplicitSchurFactor<CAMERA> implicitFactor(keys, Fblocks, E, P, b);
|
|
// JacobianFactor with same error
|
|
JacobianFactorQ<D, 2> jf(keys, Fblocks, E, P, b, model);
|
|
// JacobianFactorQR with same error
|
|
JacobianFactorQR<D, 2> jqr(keys, Fblocks, E, P, b, model);
|
|
// Hessian
|
|
HessianFactor hessianFactor(jqr);
|
|
|
|
#define TIME(label,factor,xx,yy) {\
|
|
for (size_t t = 0; t < N; t++) \
|
|
factor.multiplyHessianAdd(alpha, xx, yy);\
|
|
gttic_(label);\
|
|
for (size_t t = 0; t < N; t++) {\
|
|
factor.multiplyHessianAdd(alpha, xx, yy);\
|
|
}\
|
|
gttoc_(label);\
|
|
tictoc_getNode(timer, label)\
|
|
os << timer->secs()/NUM_ITERATIONS << ", ";\
|
|
}
|
|
|
|
#ifdef SLOW
|
|
TIME(Implicit, implicitFactor, xvalues, yvalues)
|
|
TIME(Jacobian, jf, xvalues, yvalues)
|
|
TIME(JacobianQR, jqr, xvalues, yvalues)
|
|
#endif
|
|
|
|
#ifdef HESSIAN
|
|
TIME(Hessian, hessianFactor, xvalues, yvalues)
|
|
#endif
|
|
|
|
#ifdef OVERHEAD
|
|
DummyFactor<D> dummy(Fblocks, E, P, b);
|
|
TIME(Overhead,dummy,xvalues,yvalues)
|
|
#endif
|
|
|
|
#ifdef RAW
|
|
{ // Raw memory Version
|
|
FastVector < Key > keys;
|
|
for (size_t i = 0; i < m; i++)
|
|
keys += i;
|
|
Vector x = xvalues.vector(keys);
|
|
double* xdata = x.data();
|
|
|
|
// create a y
|
|
Vector y = zero(m * D);
|
|
TIME(RawImplicit, implicitFactor, xdata, y.data())
|
|
TIME(RawJacobianQ, jf, xdata, y.data())
|
|
TIME(RawJacobianQR, jqr, xdata, y.data())
|
|
}
|
|
#endif
|
|
|
|
os << m << endl;
|
|
|
|
} // timeAll
|
|
|
|
/*************************************************************************************/
|
|
int main(void) {
|
|
#ifdef SLOW
|
|
os << "Implicit,";
|
|
os << "JacobianQ,";
|
|
os << "JacobianQR,";
|
|
#endif
|
|
#ifdef HESSIAN
|
|
os << "Hessian,";
|
|
#endif
|
|
#ifdef OVERHEAD
|
|
os << "Overhead,";
|
|
#endif
|
|
#ifdef RAW
|
|
os << "RawImplicit,";
|
|
os << "RawJacobianQ,";
|
|
os << "RawJacobianQR,";
|
|
#endif
|
|
os << "m" << endl;
|
|
// define images
|
|
vector < size_t > ms;
|
|
// ms += 2;
|
|
// ms += 3, 4, 5, 6, 7, 8, 9, 10;
|
|
// ms += 11,12,13,14,15,16,17,18,19;
|
|
// ms += 20, 30, 40, 50;
|
|
// ms += 20,30,40,50,60,70,80,90,100;
|
|
for (size_t m = 2; m <= 50; m += 2)
|
|
ms += m;
|
|
//for (size_t m=10;m<=100;m+=10) ms += m;
|
|
// loop over number of images
|
|
BOOST_FOREACH(size_t m,ms)
|
|
timeAll<PinholePose<Cal3Bundler> >(m, NUM_ITERATIONS);
|
|
}
|
|
|
|
//*************************************************************************************
|