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 timeGaussianFactorGraph.cpp
14 * @brief Time elimination with simple Kalman Smoothing example
15 * @author Frank Dellaert
16 */
17
18#include <time.h>
19#include <CppUnitLite/TestHarness.h>
20#include <tests/smallExample.h>
21
22using namespace std;
23using namespace gtsam;
24using namespace example;
25
26/* ************************************************************************* */
27// Create a Kalman smoother for t=1:T and optimize
28double 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: 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
40double 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
51double timePlanarSmootherEliminate(int N, bool old = true) {
52 GaussianFactorGraph fg = planarGraph(N).first;
53 clock_t start = clock();
54 fg.eliminateMultifrontal();
55 clock_t end = clock ();
56 double dif = (double)(end - start) / CLOCKS_PER_SEC;
57 return dif;
58}
59
60///* ************************************************************************* */
61//// Create a planar factor graph and join factors until matrix formation
62//// This variation uses the original join factors approach
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
93///* ************************************************************************* */
94//// Create a planar factor graph and join factors until matrix formation
95//// This variation uses the single-allocate version to create the matrix
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/* ************************************************************************* */
117TEST(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(T: 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
154int grid_size = 100;
155
156/* ************************************************************************* */
157TEST(timeGaussianFactorGraph, planar_old)
158{
159 cout << "Timing planar - original version" << endl;
160 double time = timePlanarSmoother(N: grid_size);
161 cout << "timeGaussianFactorGraph : " << time << endl;
162 //DOUBLES_EQUAL(5.97,time,0.1);
163}
164
165/* ************************************************************************* */
166TEST(timeGaussianFactorGraph, planar_new)
167{
168 cout << "Timing planar - new version" << endl;
169 double time = timePlanarSmoother(N: grid_size, old: false);
170 cout << "timeGaussianFactorGraph : " << time << endl;
171 //DOUBLES_EQUAL(5.97,time,0.1);
172}
173
174/* ************************************************************************* */
175TEST(timeGaussianFactorGraph, planar_eliminate_old)
176{
177 cout << "Timing planar Eliminate - original version" << endl;
178 double time = timePlanarSmootherEliminate(N: grid_size);
179 cout << "timeGaussianFactorGraph : " << time << endl;
180 //DOUBLES_EQUAL(5.97,time,0.1);
181}
182
183/* ************************************************************************* */
184TEST(timeGaussianFactorGraph, planar_eliminate_new)
185{
186 cout << "Timing planar Eliminate - new version" << endl;
187 double time = timePlanarSmootherEliminate(N: grid_size, old: false);
188 cout << "timeGaussianFactorGraph : " << time << endl;
189 //DOUBLES_EQUAL(5.97,time,0.1);
190}
191
192//size_t reps = 1000;
193///* ************************************************************************* */
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//
202///* ************************************************************************* */
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/* ************************************************************************* */
213int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
214/* ************************************************************************* */
215