timeSchurFactors.cpp
Go to the documentation of this file.
1 
8 #include "DummyFactor.h"
9 #include <gtsam/base/timing.h>
10 
16 
17 #include <fstream>
18 
19 using namespace std;
20 using namespace gtsam;
21 
22 #define SLOW
23 #define RAW
24 #define HESSIAN
25 #define NUM_ITERATIONS 1000
26 
27 // Create CSV file for results
28 ofstream os("timeSchurFactors.csv");
29 
30 /*************************************************************************************/
31 template<typename CAMERA>
32 void 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;
40  vector <Matrix2D, Eigen::aligned_allocator<Matrix2D>> Fblocks;
41  for (size_t i = 0; i < m; i++) {
42  keys.push_back(i);
43  Fblocks.push_back((i + 1) * Matrix::Ones(2, 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 > (2 * i, 0) = Matrix::Ones(2, 3);
50 
51  // Calculate point covariance
52  Matrix P = (E.transpose() * E).inverse();
53 
54  // RHS and sigmas
55  const Vector b = Vector::Constant(2*m,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(i, Vector::Constant(D,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
103  for (size_t i = 0; i < m; i++)
104  keys.push_back(i);
105  Vector x = xvalues.vector(keys);
106  double* xdata = x.data();
107 
108  // create a y
109  Vector y = Vector::Zero(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 /*************************************************************************************/
121 int 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(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 //*************************************************************************************
timing.h
Timing utilities.
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:88
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
TIME
#define TIME(label, factor, xx, yy)
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::RegularImplicitSchurFactor
Definition: RegularImplicitSchurFactor.h:39
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
timeAll
void timeAll(size_t m, size_t N)
Definition: timeSchurFactors.cpp:32
gtsam::DummyFactor
Definition: gtsam_unstable/slam/DummyFactor.h:17
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
pybind_wrapper_test_script.dummy
dummy
Definition: pybind_wrapper_test_script.py:42
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
y
Scalar * y
Definition: level1_cplx_impl.h:124
E
DiscreteKey E(5, 2)
gtsam
traits
Definition: chartTesting.h:28
ms
static const double ms
Definition: TimeOfArrivalExample.cpp:33
std
Definition: BFloat16.h:88
P
static double P[]
Definition: ellpe.c:68
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
RegularImplicitSchurFactor.h
A subclass of GaussianFactor specialized to structureless SFM.
N
#define N
Definition: igam.h:9
gtsam::JacobianFactorQ
Definition: JacobianFactorQ.h:27
gtsam::JacobianFactorQR
Definition: JacobianFactorQR.h:21
main
int main(void)
Definition: timeSchurFactors.cpp:121
Cal3Bundler.h
Calibration used by Bundler.
JacobianFactorQR.h
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
NUM_ITERATIONS
#define NUM_ITERATIONS
Definition: timeSchurFactors.cpp:25
PinholePose.h
Pinhole camera with known calibration.
DummyFactor.h
Just to help in timing overhead.
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:173
JacobianFactorQ.h


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:10:51