Finalized batch and incremental timing scripts
Richard Roberts
parent
82f98fe1fb
commit
9793f8b7f7
|
|
@ -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 <gtsam/base/timing.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
|
||||
cout << "Loading data..." << endl;
|
||||
try {
|
||||
|
||||
string datasetFile = findExampleDataFile("w10000-odom");
|
||||
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
|
||||
load2D(datasetFile);
|
||||
cout << "Loading data..." << endl;
|
||||
|
||||
NonlinearFactorGraph graph = *data.first;
|
||||
Values initial = *data.second;
|
||||
string datasetFile = findExampleDataFile("w10000-odom");
|
||||
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);
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,12 +23,34 @@
|
|||
#include <gtsam/nonlinear/ISAM2.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 gtsam;
|
||||
using namespace gtsam::symbol_shorthand;
|
||||
|
||||
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) {
|
||||
// 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue