44 lines
1.3 KiB
C++
44 lines
1.3 KiB
C++
/**
|
|
* @file timeGaussianFactorGraph.cpp
|
|
* @brief Time elimination with simple Kalman Smoothing example
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <time.h>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include "smallExample.h"
|
|
#include "Ordering.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
// Create a Kalman smoother for t=1:T and optimize
|
|
double timeKalmanSmoother(int T) {
|
|
GaussianFactorGraph smoother = createSmoother(T);
|
|
Ordering ordering;
|
|
for (int t = 1; t <= T; t++) ordering.push_back(symbol('x',t));
|
|
clock_t start = clock();
|
|
smoother.optimize(ordering);
|
|
clock_t end = clock ();
|
|
double dif = (double)(end - start) / CLOCKS_PER_SEC;
|
|
return dif;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(timeGaussianFactorGraph, linearTime)
|
|
{
|
|
int T = 1000;
|
|
double time1 = timeKalmanSmoother( T); // cout << time1 << endl;
|
|
double time2 = timeKalmanSmoother(2*T); // cout << time2 << endl;
|
|
DOUBLES_EQUAL(2*time1,time2,0.001);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
TestRegistry::runAllTests(tr);
|
|
return 0;
|
|
}
|
|
/* ************************************************************************* */
|