timeGaussianFactor.cpp
Go to the documentation of this file.
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 
18 #include <time.h>
19 
20 /*STL/C++*/
21 #include <iostream>
22 using namespace std;
23 
24 
25 #include <gtsam/base/Matrix.h>
30 
31 using namespace gtsam;
32 
33 static 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 
50 int 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(8,1));
102  long timeLog = clock();
103  int n = 1000000;
106 
107  for(int i = 0; i < n; i++)
108  std::tie(conditional, factor) =
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 }
_x1_
static const Key _x1_
Definition: timeGaussianFactor.cpp:33
GaussianConditional.h
Conditional Gaussian Base class.
Matrix.h
typedef and functions to augment Eigen's MatrixXd
simple_graph::b2
Vector2 b2(4, -5)
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::JacobianFactor::eliminate
std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > eliminate(const Ordering &keys)
Definition: JacobianFactor.cpp:760
n
int n
Definition: BiCGSTAB_simple.cpp:1
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
main
int main()
Definition: timeGaussianFactor.cpp:50
JacobianFactor.h
gtsam
traits
Definition: SFMdata.h:40
NoiseModel.h
std
Definition: BFloat16.h:88
gtsam::GaussianConditional::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:46
_l1_
static const Key _l1_
Definition: timeGaussianFactor.cpp:33
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
_x2_
static const Key _x2_
Definition: timeGaussianFactor.cpp:33


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:08