timeGaussianFactorGraph.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>
20 #include <tests/smallExample.h>
21 
22 using namespace std;
23 using namespace gtsam;
24 using namespace example;
25 
26 /* ************************************************************************* */
27 // Create a Kalman smoother for t=1:T and optimize
28 double timeKalmanSmoother(int T) {
29  GaussianFactorGraph smoother = createSmoother(T);
30  clock_t start = clock();
31  // Keys will come out sorted since keys() returns a set
32  smoother.optimize(Ordering(smoother.keys()));
33  clock_t end = clock ();
34  double dif = (double)(end - start) / CLOCKS_PER_SEC;
35  return dif;
36 }
37 
38 /* ************************************************************************* */
39 // Create a planar factor graph and optimize
40 double timePlanarSmoother(int N, bool old = true) {
41  GaussianFactorGraph fg = planarGraph(N).first;
42  clock_t start = clock();
43  fg.optimize();
44  clock_t end = clock ();
45  double dif = (double)(end - start) / CLOCKS_PER_SEC;
46  return dif;
47 }
48 
49 /* ************************************************************************* */
50 // Create a planar factor graph and eliminate
51 double timePlanarSmootherEliminate(int N, bool old = true) {
52  GaussianFactorGraph fg = planarGraph(N).first;
53  clock_t start = clock();
55  clock_t end = clock ();
56  double dif = (double)(end - start) / CLOCKS_PER_SEC;
57  return dif;
58 }
59 
63 //double timePlanarSmootherJoinAug(int N, size_t reps) {
64 // GaussianFactorGraph fgBase;
65 // VectorValues config;
66 // std::tie(fgBase,config) = planarGraph(N);
67 // Ordering ordering = fgBase.getOrdering();
68 // Symbol key = ordering.front();
69 //
70 // clock_t start = clock();
71 //
72 // for (size_t i = 0; i<reps; ++i) {
73 // // setup
74 // GaussianFactorGraph fg(fgBase);
75 //
76 // // combine some factors
77 // GaussianFactor::shared_ptr joint_factor = removeAndCombineFactors(fg,key);
78 //
79 // // create an internal ordering to render Ab
80 // Ordering render;
81 // render += key;
82 // for(const Symbol& k: joint_factor->keys())
83 // if (k != key) render += k;
84 //
85 // Matrix Ab = joint_factor->matrix_augmented(render,false);
86 // }
87 //
88 // clock_t end = clock ();
89 // double dif = (double)(end - start) / CLOCKS_PER_SEC;
90 // return dif;
91 //}
92 
96 //double timePlanarSmootherCombined(int N, size_t reps) {
97 // GaussianFactorGraph fgBase;
98 // VectorValues config;
99 // std::tie(fgBase,config) = planarGraph(N);
100 // Ordering ordering = fgBase.getOrdering();
101 // Symbol key = ordering.front();
102 //
103 // clock_t start = clock();
104 //
105 // for (size_t i = 0; i<reps; ++i) {
106 // GaussianFactorGraph fg(fgBase);
107 // fg.eliminateOneMatrixJoin(key);
108 // }
109 //
110 // clock_t end = clock ();
111 // double dif = (double)(end - start) / CLOCKS_PER_SEC;
112 // return dif;
113 //}
114 
115 
116 /* ************************************************************************* */
117 TEST(timeGaussianFactorGraph, linearTime)
118 {
119  // Original T = 1000;
120 
121  // Alex's Results
122  // T = 100000
123  // 1907 (init) : T - 1.65, 2T = 3.28
124  // int->size_t : T - 1.63, 2T = 3.27
125  // 2100 : T - 1.52, 2T = 2.96
126 
127  int T = 100000;
128  double time1 = timeKalmanSmoother( T); cout << "timeKalmanSmoother( T): " << time1;
129  double time2 = timeKalmanSmoother(2*T); cout << " (2*T): " << time2 << endl;
130  DOUBLES_EQUAL(2*time1,time2,0.2);
131 }
132 
133 
134 // Timing with N = 30
135 // 1741: 8.12, 8.12, 8.12, 8.14, 8.16
136 // 1742: 5.97, 5.97, 5.97, 5.99, 6.02
137 // 1746: 5.96, 5.96, 5.97, 6.00, 6.04
138 // 1748: 5.91, 5.92, 5.93, 5.95, 5.96
139 // 1839: 0.206956 0.206939 0.206213 0.206092 0.206780 // colamd !!!!
140 
141 // Alex's Machine
142 // Initial:
143 // 1907 (N = 30) : 0.14
144 // (N = 100) : 16.36
145 // Improved (int->size_t)
146 // (N = 100) : 15.39
147 // Using GSL/BLAS for updateAb : 12.87
148 // Using NoiseQR : 16.33
149 // Using correct system : 10.00
150 
151 // Switch to 100*100 grid = 10K poses
152 // 1879: 15.6498 15.3851 15.5279
153 
154 int grid_size = 100;
155 
156 /* ************************************************************************* */
157 TEST(timeGaussianFactorGraph, planar_old)
158 {
159  cout << "Timing planar - original version" << endl;
161  cout << "timeGaussianFactorGraph : " << time << endl;
162  //DOUBLES_EQUAL(5.97,time,0.1);
163 }
164 
165 /* ************************************************************************* */
166 TEST(timeGaussianFactorGraph, planar_new)
167 {
168  cout << "Timing planar - new version" << endl;
169  double time = timePlanarSmoother(grid_size, false);
170  cout << "timeGaussianFactorGraph : " << time << endl;
171  //DOUBLES_EQUAL(5.97,time,0.1);
172 }
173 
174 /* ************************************************************************* */
175 TEST(timeGaussianFactorGraph, planar_eliminate_old)
176 {
177  cout << "Timing planar Eliminate - original version" << endl;
179  cout << "timeGaussianFactorGraph : " << time << endl;
180  //DOUBLES_EQUAL(5.97,time,0.1);
181 }
182 
183 /* ************************************************************************* */
184 TEST(timeGaussianFactorGraph, planar_eliminate_new)
185 {
186  cout << "Timing planar Eliminate - new version" << endl;
187  double time = timePlanarSmootherEliminate(grid_size, false);
188  cout << "timeGaussianFactorGraph : " << time << endl;
189  //DOUBLES_EQUAL(5.97,time,0.1);
190 }
191 
192 //size_t reps = 1000;
194 //TEST(timeGaussianFactorGraph, planar_join_old)
195 //{
196 // cout << "Timing planar join - old" << endl;
197 // double time = timePlanarSmootherJoinAug(size, reps);
198 // cout << "timePlanarSmootherJoinAug " << size << " : " << time << endl;
199 // //DOUBLES_EQUAL(5.97,time,0.1);
200 //}
201 //
203 //TEST(timeGaussianFactorGraph, planar_join_new)
204 //{
205 // cout << "Timing planar join - new" << endl;
206 // double time = timePlanarSmootherCombined(size, reps);
207 // cout << "timePlanarSmootherCombined " << size << " : " << time << endl;
208 // //DOUBLES_EQUAL(5.97,time,0.1);
209 //}
210 
211 
212 /* ************************************************************************* */
213 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
214 /* ************************************************************************* */
GaussianFactorGraph createSmoother(int T)
Definition: smallExample.h:465
static int runAllTests(TestResult &result)
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
Definition: BFloat16.h:88
#define N
Definition: gksort.c:12
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
#define time
TEST(timeGaussianFactorGraph, linearTime)
double timePlanarSmoother(int N, bool old=true)
traits
Definition: chartTesting.h:28
std::pair< GaussianFactorGraph, VectorValues > planarGraph(size_t N)
Definition: smallExample.h:626
static EIGEN_DEPRECATED const end_t end
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Create small example with two poses and one landmark.
double timeKalmanSmoother(int T)
double timePlanarSmootherEliminate(int N, bool old=true)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:01