12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Finalized batch and incremental timing scripts

release/4.3a0
Richard Roberts 2012-10-08 22:40:47 +00:00
parent 82f98fe1fb
commit 9793f8b7f7
2 changed files with 123 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
tests/timeBatch.cpp
View File

@ -1,54 +1,76 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file timeBatch.cpp * @file timeBatch.cpp
* @brief Overall timing tests for batch solving * @brief Overall timing tests for batch solving
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
int main(int argc, char *argv[]) { int main(int argc, char *argv[]) {
cout << "Loading data..." << endl; try {
string datasetFile = findExampleDataFile("w10000-odom"); cout << "Loading data..." << endl;
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
load2D(datasetFile);
NonlinearFactorGraph graph = *data.first; string datasetFile = findExampleDataFile("w10000-odom");
Values initial = *data.second; std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
load2D(datasetFile);
cout << "Optimizing..." << endl; NonlinearFactorGraph graph = *data.first;
Values initial = *data.second;
gttic_(Create_optimizer); cout << "Optimizing..." << endl;
LevenbergMarquardtOptimizer optimizer(graph, initial);
gttoc_(Create_optimizer); gttic_(Create_optimizer);
tictoc_print_(); LevenbergMarquardtOptimizer optimizer(graph, initial);
double lastError = optimizer.error(); gttoc_(Create_optimizer);
do {
gttic_(Iterate_optimizer);
optimizer.iterate();
gttoc_(Iterate_optimizer);
tictoc_finishedIteration_();
tictoc_print_(); tictoc_print_();
cout << "Error: " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl; double lastError = optimizer.error();
} while(!checkConvergence(optimizer.params().relativeErrorTol, do {
optimizer.params().absoluteErrorTol, optimizer.params().errorTol, gttic_(Iterate_optimizer);
lastError, optimizer.error(), optimizer.params().verbosity)); optimizer.iterate();
gttoc_(Iterate_optimizer);
tictoc_finishedIteration_();
tictoc_print_();
cout << "Error: " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl;
break;
} while(!checkConvergence(optimizer.params().relativeErrorTol,
optimizer.params().absoluteErrorTol, optimizer.params().errorTol,
lastError, optimizer.error(), optimizer.params().verbosity));
// Compute marginals
Marginals marginals(graph, optimizer.values());
int i=0;
BOOST_FOREACH(Key key, initial.keys()) {
gttic_(marginalInformation);
Matrix info = marginals.marginalInformation(key);
gttoc_(marginalInformation);
tictoc_finishedIteration_();
if(i % 1000 == 0)
tictoc_print_();
++i;
}
} catch(std::exception& e) {
cout << e.what() << endl;
return 1;
}
return 0; return 0;
} }

80
tests/timeIncremental.cpp
View File

@ -23,12 +23,34 @@
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Marginals.h>
#include <fstream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::symbol_shorthand; using namespace gtsam::symbol_shorthand;
typedef Pose2 Pose; typedef Pose2 Pose;
typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
BOOST_CLASS_EXPORT(Value);
BOOST_CLASS_EXPORT(Pose);
BOOST_CLASS_EXPORT(NonlinearFactor);
BOOST_CLASS_EXPORT(NoiseModelFactor);
BOOST_CLASS_EXPORT(NM1);
BOOST_CLASS_EXPORT(NM2);
BOOST_CLASS_EXPORT(BetweenFactor<Pose>);
BOOST_CLASS_EXPORT(PriorFactor<Pose>);
BOOST_CLASS_EXPORT(noiseModel::Base);
BOOST_CLASS_EXPORT(noiseModel::Isotropic);
BOOST_CLASS_EXPORT(noiseModel::Gaussian);
BOOST_CLASS_EXPORT(noiseModel::Diagonal);
BOOST_CLASS_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables) // Compute degrees of freedom (observations - variables)
// In ocaml, +1 was added to the observations to account for the prior, but // In ocaml, +1 was added to the observations to account for the prior, but
@ -137,17 +159,55 @@ int main(int argc, char *argv[]) {
} }
} }
try {
{
std::ofstream writerStream("incremental_init", ios::binary);
boost::archive::binary_oarchive writer(writerStream);
writer << isam2.calculateEstimate();
writerStream.close();
}
{
std::ofstream writerStream("incremental_graph", ios::binary);
boost::archive::binary_oarchive writer(writerStream);
writer << isam2.getFactorsUnsafe();
writerStream.close();
}
} catch(std::exception& e) {
cout << e.what() << endl;
}
NonlinearFactorGraph graph;
Values values;
//{
// std::ifstream readerStream("incremental_init", ios::binary);
// boost::archive::binary_iarchive reader(readerStream);
// reader >> values;
//}
//{
// std::ifstream readerStream("incremental_graph", ios::binary);
// boost::archive::binary_iarchive reader(readerStream);
// reader >> graph;
//}
graph = isam2.getFactorsUnsafe();
values = isam2.calculateEstimate();
// Compute marginals // Compute marginals
Marginals marginals(isam2.getFactorsUnsafe(), isam2.calculateEstimate()); try {
int i=0; Marginals marginals(graph, values);
BOOST_FOREACH(Key key, initial.keys()) { int i=0;
gttic_(marginalInformation); BOOST_FOREACH(Key key, values.keys()) {
Matrix info = marginals.marginalInformation(key); gttic_(marginalInformation);
gttoc_(marginalInformation); Matrix info = marginals.marginalInformation(key);
tictoc_finishedIteration_(); gttoc_(marginalInformation);
if(i % 1000 == 0) tictoc_finishedIteration_();
tictoc_print_(); if(i % 1000 == 0)
++i; tictoc_print_();
++i;
}
} catch(std::exception& e) {
cout << e.what() << endl;
} }
return 0; return 0;