1/**
2 * @file timeSchurFactors.cpp
3 * @brief Time various Schur-complement Jacobian factors
4 * @author Frank Dellaert
5 * @date Oct 27, 2013
6 */
7
8#include "DummyFactor.h"
9#include <gtsam/base/timing.h>
10
11#include <gtsam/slam/JacobianFactorQ.h>
12#include "gtsam/slam/JacobianFactorQR.h"
13#include <gtsam/slam/RegularImplicitSchurFactor.h>
14#include <gtsam/geometry/Cal3Bundler.h>
15#include <gtsam/geometry/PinholePose.h>
16
17#include <fstream>
18
19using namespace std;
20using namespace gtsam;
21
22#define SLOW
23#define RAW
24#define HESSIAN
25#define NUM_ITERATIONS 1000
26
27// Create CSV file for results
28ofstream os("timeSchurFactors.csv");
29
30/*************************************************************************************/
31template<typename CAMERA>
32void timeAll(size_t m, size_t N) {
33
34 cout << m << endl;
35
36 // create F
37 static const int D = CAMERA::dimension;
38 typedef Eigen::Matrix<double, 2, D> Matrix2D;
39 KeyVector keys;
40 vector <Matrix2D, Eigen::aligned_allocator<Matrix2D>> Fblocks;
41 for (size_t i = 0; i < m; i++) {
42 keys.push_back(x: i);
43 Fblocks.push_back((i + 1) * Matrix::Ones(rows: 2, cols: D));
44 }
45
46 // create E
47 Matrix E(2 * m, 3);
48 for (size_t i = 0; i < m; i++)
49 E.block < 2, 3 > (startRow: 2 * i, startCol: 0) = Matrix::Ones(rows: 2, cols: 3);
50
51 // Calculate point covariance
52 Matrix P = (E.transpose() * E).inverse();
53
54 // RHS and sigmas
55 const Vector b = Vector::Constant(size: 2*m,value: 1);
56 const SharedDiagonal model;
57
58 // parameters for multiplyHessianAdd
59 double alpha = 0.5;
60 VectorValues xvalues, yvalues;
61 for (size_t i = 0; i < m; i++)
62 xvalues.insert(j: i, value: Vector::Constant(size: D,value: 2));
63
64 // Implicit
65 RegularImplicitSchurFactor<CAMERA> implicitFactor(keys, Fblocks, E, P, b);
66 // JacobianFactor with same error
67 JacobianFactorQ<D, 2> jf(keys, Fblocks, E, P, b, model);
68 // JacobianFactorQR with same error
69 JacobianFactorQR<D, 2> jqr(keys, Fblocks, E, P, b, model);
70 // Hessian
71 HessianFactor hessianFactor(jqr);
72
73#define TIME(label,factor,xx,yy) {\
74 for (size_t t = 0; t < N; t++) \
75 factor.multiplyHessianAdd(alpha, xx, yy);\
76 gttic_(label);\
77 for (size_t t = 0; t < N; t++) {\
78 factor.multiplyHessianAdd(alpha, xx, yy);\
79 }\
80 gttoc_(label);\
81 tictoc_getNode(timer, label)\
82 os << timer->secs()/NUM_ITERATIONS << ", ";\
83 }
84
85#ifdef SLOW
86 TIME(Implicit, implicitFactor, xvalues, yvalues)
87 TIME(Jacobian, jf, xvalues, yvalues)
88 TIME(JacobianQR, jqr, xvalues, yvalues)
89#endif
90
91#ifdef HESSIAN
92 TIME(Hessian, hessianFactor, xvalues, yvalues)
93#endif
94
95#ifdef OVERHEAD
96 DummyFactor<D> dummy(Fblocks, E, P, b);
97 TIME(Overhead,dummy,xvalues,yvalues)
98#endif
99
100#ifdef RAW
101 { // Raw memory Version
102 FastVector < Key > keys;
103 for (size_t i = 0; i < m; i++)
104 keys.push_back(x: i);
105 Vector x = xvalues.vector(keys);
106 double* xdata = x.data();
107
108 // create a y
109 Vector y = Vector::Zero(size: m * D);
110 TIME(RawImplicit, implicitFactor, xdata, y.data())
111 TIME(RawJacobianQ, jf, xdata, y.data())
112 TIME(RawJacobianQR, jqr, xdata, y.data())
113 }
114#endif
115
116 os << m << endl;
117
118} // timeAll
119
120/*************************************************************************************/
121int main(void) {
122#ifdef SLOW
123 os << "Implicit,";
124 os << "JacobianQ,";
125 os << "JacobianQR,";
126#endif
127#ifdef HESSIAN
128 os << "Hessian,";
129#endif
130#ifdef OVERHEAD
131 os << "Overhead,";
132#endif
133#ifdef RAW
134 os << "RawImplicit,";
135 os << "RawJacobianQ,";
136 os << "RawJacobianQR,";
137#endif
138 os << "m" << endl;
139 // define images
140 vector < size_t > ms;
141 // ms += 2;
142 // ms += 3, 4, 5, 6, 7, 8, 9, 10;
143 // ms += 11,12,13,14,15,16,17,18,19;
144 // ms += 20, 30, 40, 50;
145 // ms += 20,30,40,50,60,70,80,90,100;
146 for (size_t m = 2; m <= 50; m += 2)
147 ms.push_back(x: m);
148 //for (size_t m=10;m<=100;m+=10) ms += m;
149 // loop over number of images
150 for(size_t m: ms)
151 timeAll<PinholePose<Cal3Bundler> >(m, NUM_ITERATIONS);
152}
153
154//*************************************************************************************
155