218 lines
7.1 KiB
C++
218 lines
7.1 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file timeGaussianFactorGraph.cpp
|
|
* @brief Time elimination with simple Kalman Smoothing example
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <time.h>
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/assign/std/list.hpp> // for operator += in Ordering
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <tests/smallExample.h>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
using namespace example;
|
|
using namespace boost::assign;
|
|
|
|
/* ************************************************************************* */
|
|
// Create a Kalman smoother for t=1:T and optimize
|
|
double timeKalmanSmoother(int T) {
|
|
GaussianFactorGraph smoother = createSmoother(T);
|
|
clock_t start = clock();
|
|
// Keys will come out sorted since keys() returns a set
|
|
smoother.optimize(Ordering(smoother.keys()));
|
|
clock_t end = clock ();
|
|
double dif = (double)(end - start) / CLOCKS_PER_SEC;
|
|
return dif;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Create a planar factor graph and optimize
|
|
double timePlanarSmoother(int N, bool old = true) {
|
|
GaussianFactorGraph fg = planarGraph(N).get<0>();
|
|
clock_t start = clock();
|
|
fg.optimize();
|
|
clock_t end = clock ();
|
|
double dif = (double)(end - start) / CLOCKS_PER_SEC;
|
|
return dif;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Create a planar factor graph and eliminate
|
|
double timePlanarSmootherEliminate(int N, bool old = true) {
|
|
GaussianFactorGraph fg = planarGraph(N).get<0>();
|
|
clock_t start = clock();
|
|
fg.eliminateMultifrontal();
|
|
clock_t end = clock ();
|
|
double dif = (double)(end - start) / CLOCKS_PER_SEC;
|
|
return dif;
|
|
}
|
|
|
|
///* ************************************************************************* */
|
|
//// Create a planar factor graph and join factors until matrix formation
|
|
//// This variation uses the original join factors approach
|
|
//double timePlanarSmootherJoinAug(int N, size_t reps) {
|
|
// GaussianFactorGraph fgBase;
|
|
// VectorValues config;
|
|
// boost::tie(fgBase,config) = planarGraph(N);
|
|
// Ordering ordering = fgBase.getOrdering();
|
|
// Symbol key = ordering.front();
|
|
//
|
|
// clock_t start = clock();
|
|
//
|
|
// for (size_t i = 0; i<reps; ++i) {
|
|
// // setup
|
|
// GaussianFactorGraph fg(fgBase);
|
|
//
|
|
// // combine some factors
|
|
// GaussianFactor::shared_ptr joint_factor = removeAndCombineFactors(fg,key);
|
|
//
|
|
// // create an internal ordering to render Ab
|
|
// Ordering render;
|
|
// render += key;
|
|
// for(const Symbol& k: joint_factor->keys())
|
|
// if (k != key) render += k;
|
|
//
|
|
// Matrix Ab = joint_factor->matrix_augmented(render,false);
|
|
// }
|
|
//
|
|
// clock_t end = clock ();
|
|
// double dif = (double)(end - start) / CLOCKS_PER_SEC;
|
|
// return dif;
|
|
//}
|
|
|
|
///* ************************************************************************* */
|
|
//// Create a planar factor graph and join factors until matrix formation
|
|
//// This variation uses the single-allocate version to create the matrix
|
|
//double timePlanarSmootherCombined(int N, size_t reps) {
|
|
// GaussianFactorGraph fgBase;
|
|
// VectorValues config;
|
|
// boost::tie(fgBase,config) = planarGraph(N);
|
|
// Ordering ordering = fgBase.getOrdering();
|
|
// Symbol key = ordering.front();
|
|
//
|
|
// clock_t start = clock();
|
|
//
|
|
// for (size_t i = 0; i<reps; ++i) {
|
|
// GaussianFactorGraph fg(fgBase);
|
|
// fg.eliminateOneMatrixJoin(key);
|
|
// }
|
|
//
|
|
// clock_t end = clock ();
|
|
// double dif = (double)(end - start) / CLOCKS_PER_SEC;
|
|
// return dif;
|
|
//}
|
|
|
|
|
|
/* ************************************************************************* */
|
|
TEST(timeGaussianFactorGraph, linearTime)
|
|
{
|
|
// Original T = 1000;
|
|
|
|
// Alex's Results
|
|
// T = 100000
|
|
// 1907 (init) : T - 1.65, 2T = 3.28
|
|
// int->size_t : T - 1.63, 2T = 3.27
|
|
// 2100 : T - 1.52, 2T = 2.96
|
|
|
|
int T = 100000;
|
|
double time1 = timeKalmanSmoother( T); cout << "timeKalmanSmoother( T): " << time1;
|
|
double time2 = timeKalmanSmoother(2*T); cout << " (2*T): " << time2 << endl;
|
|
DOUBLES_EQUAL(2*time1,time2,0.2);
|
|
}
|
|
|
|
|
|
// Timing with N = 30
|
|
// 1741: 8.12, 8.12, 8.12, 8.14, 8.16
|
|
// 1742: 5.97, 5.97, 5.97, 5.99, 6.02
|
|
// 1746: 5.96, 5.96, 5.97, 6.00, 6.04
|
|
// 1748: 5.91, 5.92, 5.93, 5.95, 5.96
|
|
// 1839: 0.206956 0.206939 0.206213 0.206092 0.206780 // colamd !!!!
|
|
|
|
// Alex's Machine
|
|
// Initial:
|
|
// 1907 (N = 30) : 0.14
|
|
// (N = 100) : 16.36
|
|
// Improved (int->size_t)
|
|
// (N = 100) : 15.39
|
|
// Using GSL/BLAS for updateAb : 12.87
|
|
// Using NoiseQR : 16.33
|
|
// Using correct system : 10.00
|
|
|
|
// Switch to 100*100 grid = 10K poses
|
|
// 1879: 15.6498 15.3851 15.5279
|
|
|
|
int size = 100;
|
|
|
|
/* ************************************************************************* */
|
|
TEST(timeGaussianFactorGraph, planar_old)
|
|
{
|
|
cout << "Timing planar - original version" << endl;
|
|
double time = timePlanarSmoother(size);
|
|
cout << "timeGaussianFactorGraph : " << time << endl;
|
|
//DOUBLES_EQUAL(5.97,time,0.1);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(timeGaussianFactorGraph, planar_new)
|
|
{
|
|
cout << "Timing planar - new version" << endl;
|
|
double time = timePlanarSmoother(size, false);
|
|
cout << "timeGaussianFactorGraph : " << time << endl;
|
|
//DOUBLES_EQUAL(5.97,time,0.1);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(timeGaussianFactorGraph, planar_eliminate_old)
|
|
{
|
|
cout << "Timing planar Eliminate - original version" << endl;
|
|
double time = timePlanarSmootherEliminate(size);
|
|
cout << "timeGaussianFactorGraph : " << time << endl;
|
|
//DOUBLES_EQUAL(5.97,time,0.1);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(timeGaussianFactorGraph, planar_eliminate_new)
|
|
{
|
|
cout << "Timing planar Eliminate - new version" << endl;
|
|
double time = timePlanarSmootherEliminate(size, false);
|
|
cout << "timeGaussianFactorGraph : " << time << endl;
|
|
//DOUBLES_EQUAL(5.97,time,0.1);
|
|
}
|
|
|
|
//size_t reps = 1000;
|
|
///* ************************************************************************* */
|
|
//TEST(timeGaussianFactorGraph, planar_join_old)
|
|
//{
|
|
// cout << "Timing planar join - old" << endl;
|
|
// double time = timePlanarSmootherJoinAug(size, reps);
|
|
// cout << "timePlanarSmootherJoinAug " << size << " : " << time << endl;
|
|
// //DOUBLES_EQUAL(5.97,time,0.1);
|
|
//}
|
|
//
|
|
///* ************************************************************************* */
|
|
//TEST(timeGaussianFactorGraph, planar_join_new)
|
|
//{
|
|
// cout << "Timing planar join - new" << endl;
|
|
// double time = timePlanarSmootherCombined(size, reps);
|
|
// cout << "timePlanarSmootherCombined " << size << " : " << time << endl;
|
|
// //DOUBLES_EQUAL(5.97,time,0.1);
|
|
//}
|
|
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|