1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file timeGaussianFactor.cpp
14 * @brief time JacobianFactor.eliminate
15 * @author Alireza Fathi
16 */
17
18#include <time.h>
19
20/*STL/C++*/
21#include <iostream>
22using namespace std;
23
24
25#include <gtsam/base/Matrix.h>
26#include <gtsam/linear/JacobianFactor.h>
27#include <gtsam/linear/JacobianFactor.h>
28#include <gtsam/linear/GaussianConditional.h>
29#include <gtsam/linear/NoiseModel.h>
30
31using namespace gtsam;
32
33static const Key _x1_=1, _x2_=2, _l1_=3;
34
35/*
36 * Alex's Machine
37 * Results for Eliminate:
38 * Initial (1891): 17.91 sec, 55834.7 calls/sec
39 * NoiseQR : 11.69 sec
40 *
41 * Results for matrix_augmented:
42 * Initial (1891) : 0.85 sec, 1.17647e+06 calls/sec
43 * int->size_t Version : 8.45 sec (for n1 reps) with memcpy version of collect()
44 * w/ original collect(): 8.73 sec (for n1 reps)
45 * b memcpy Version : 8.64 sec (for n1 reps) with original version of collect()
46 * w/ memcpy collect() : 8.40 sec (for n1 reps)
47 * Rev 2100 : 8.15 sec
48 */
49
50int main()
51{
52 // create a linear factor
53 Matrix Ax2 = (Matrix(8, 2) <<
54 // x2
55 -5., 0.,
56 +0.,-5.,
57 10., 0.,
58 +0.,10.,
59 -5., 0.,
60 +0.,-5.,
61 10., 0.,
62 +0.,10.
63 ).finished();
64
65 Matrix Al1 = (Matrix(8, 10) <<
66 // l1
67 5., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
68 0., 5.,1.,2.,3.,4.,5.,6.,7.,8.,
69 0., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
70 0., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
71 5., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
72 0., 5.,1.,2.,3.,4.,5.,6.,7.,8.,
73 0., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
74 0., 0.,1.,2.,3.,4.,5.,6.,7.,8.
75 ).finished();
76
77 Matrix Ax1 = (Matrix(8, 2) <<
78 // x1
79 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8.,
80 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8.,
81 -10., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
82 0.00,-10.,1.,2.,3.,4.,5.,6.,7.,8.,
83 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8.,
84 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8.,
85 -10., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
86 0.00,-10.,1.,2.,3.,4.,5.,6.,7.,8.
87 ).finished();
88
89 // and a RHS
90 Vector b2(8);
91 b2(0) = -1;
92 b2(1) = 1.5;
93 b2(2) = 2;
94 b2(3) = -1;
95 b2(4) = -1;
96 b2(5) = 1.5;
97 b2(6) = 2;
98 b2(7) = -1;
99
100 // time eliminate
101 JacobianFactor combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(dim: 8,sigma: 1));
102 long timeLog = clock();
103 int n = 1000000;
104 GaussianConditional::shared_ptr conditional;
105 JacobianFactor::shared_ptr factor;
106
107 for(int i = 0; i < n; i++)
108 std::tie(args&: conditional, args&: factor) =
109 JacobianFactor(combined).eliminate(keys: Ordering{_x2_});
110
111 long timeLog2 = clock();
112 double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
113 cout << "Single Eliminate Timing:" << endl;
114 cout << seconds << " seconds" << endl;
115 cout << ((double)n/seconds) << " calls/second" << endl;
116
117 // time matrix_augmented
118// const Ordering ordering{_x2_, _l1_, _x1_};
119// size_t n1 = 10000000;
120// timeLog = clock();
121//
122// for(size_t i = 0; i < n1; i++)
123// Matrix Ab = combined.matrix_augmented(ordering, true);
124//
125// timeLog2 = clock();
126// seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
127// cout << "matrix_augmented Timing:" << endl;
128// cout << seconds << " seconds" << endl;
129// cout << ((double)n/seconds) << " calls/second" << endl;
130
131 return 0;
132}
133