Moved from smartfactors_ceres
dellaert
parent
9d2666e56e
commit
6365dbaba5
|
|
@ -0,0 +1,56 @@
|
|||
/**
|
||||
* @file DummyFactor.h
|
||||
* @brief Just to help in timing overhead
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/RegularImplicitSchurFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* DummyFactor
|
||||
*/
|
||||
template<size_t D> //
|
||||
class DummyFactor: public RegularImplicitSchurFactor<D> {
|
||||
|
||||
public:
|
||||
|
||||
typedef Eigen::Matrix<double, 2, D> Matrix2D;
|
||||
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
|
||||
|
||||
DummyFactor() {
|
||||
}
|
||||
|
||||
DummyFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
|
||||
const Matrix3& P, const Vector& b) :RegularImplicitSchurFactor<D>(Fblocks,E,P,b)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~DummyFactor() {
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Dummy version to measure overhead of key access
|
||||
*/
|
||||
void multiplyHessian(double alpha, const VectorValues& x,
|
||||
VectorValues& y) const {
|
||||
|
||||
BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) {
|
||||
static const Vector empty;
|
||||
Key key = Fi.first;
|
||||
std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
|
||||
Vector& yi = it.first->second;
|
||||
yi = x.at(key);
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
// DummyFactor
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
/**
|
||||
* @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 <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<size_t D>
|
||||
void timeAll(size_t m, size_t N) {
|
||||
|
||||
cout << m << endl;
|
||||
|
||||
// create F
|
||||
typedef Eigen::Matrix<double, 2, D> Matrix2D;
|
||||
typedef pair<Key, Matrix2D> KeyMatrix2D;
|
||||
vector < pair<Key, Matrix2D> > Fblocks;
|
||||
for (size_t i = 0; i < m; i++)
|
||||
Fblocks.push_back(KeyMatrix2D(i, (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<D> implicitFactor(Fblocks, E, P, b);
|
||||
// JacobianFactor with same error
|
||||
JacobianFactorQ<D, 2> jf(Fblocks, E, P, b, model);
|
||||
// JacobianFactorQR with same error
|
||||
JacobianFactorQR<D, 2> jqr(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<6>(m, NUM_ITERATIONS);
|
||||
}
|
||||
|
||||
//*************************************************************************************
|
||||
Loading…
Reference in New Issue