diff --git a/tests/timeBatch.cpp b/tests/timeBatch.cpp index a8d2d9e82..a1e5f359b 100644 --- a/tests/timeBatch.cpp +++ b/tests/timeBatch.cpp @@ -1,54 +1,76 @@ /* ---------------------------------------------------------------------------- - * 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) +* 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 - * -------------------------------------------------------------------------- */ +* See LICENSE for the license information +* -------------------------------------------------------------------------- */ /** - * @file timeBatch.cpp - * @brief Overall timing tests for batch solving - * @author Richard Roberts - */ +* @file timeBatch.cpp +* @brief Overall timing tests for batch solving +* @author Richard Roberts +*/ #include #include #include +#include using namespace std; using namespace gtsam; int main(int argc, char *argv[]) { - cout << "Loading data..." << endl; + try { - string datasetFile = findExampleDataFile("w10000-odom"); - std::pair data = - load2D(datasetFile); + cout << "Loading data..." << endl; - NonlinearFactorGraph graph = *data.first; - Values initial = *data.second; + string datasetFile = findExampleDataFile("w10000-odom"); + std::pair data = + load2D(datasetFile); - cout << "Optimizing..." << endl; + NonlinearFactorGraph graph = *data.first; + Values initial = *data.second; - gttic_(Create_optimizer); - LevenbergMarquardtOptimizer optimizer(graph, initial); - gttoc_(Create_optimizer); - tictoc_print_(); - double lastError = optimizer.error(); - do { - gttic_(Iterate_optimizer); - optimizer.iterate(); - gttoc_(Iterate_optimizer); - tictoc_finishedIteration_(); + cout << "Optimizing..." << endl; + + gttic_(Create_optimizer); + LevenbergMarquardtOptimizer optimizer(graph, initial); + gttoc_(Create_optimizer); tictoc_print_(); - cout << "Error: " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl; - } while(!checkConvergence(optimizer.params().relativeErrorTol, - optimizer.params().absoluteErrorTol, optimizer.params().errorTol, - lastError, optimizer.error(), optimizer.params().verbosity)); + double lastError = optimizer.error(); + do { + gttic_(Iterate_optimizer); + 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; } diff --git a/tests/timeIncremental.cpp b/tests/timeIncremental.cpp index 15ff8acd7..433d3cd0a 100644 --- a/tests/timeIncremental.cpp +++ b/tests/timeIncremental.cpp @@ -23,12 +23,34 @@ #include #include +#include +#include +#include +#include + using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; typedef Pose2 Pose; +typedef NoiseModelFactor1 NM1; +typedef NoiseModelFactor2 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); +BOOST_CLASS_EXPORT(PriorFactor); +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) { // Compute degrees of freedom (observations - variables) // 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 - Marginals marginals(isam2.getFactorsUnsafe(), isam2.calculateEstimate()); - 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; + try { + Marginals marginals(graph, values); + int i=0; + BOOST_FOREACH(Key key, values.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 0;