75 lines
2.1 KiB
C++
75 lines
2.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 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[]) {
|
|
|
|
try {
|
|
|
|
cout << "Loading data..." << endl;
|
|
|
|
string datasetFile = findExampleDataFile("w10000");
|
|
auto [graph, initial] = load2D(datasetFile);
|
|
graph->addPrior(0, initial->at<Pose2>(0), noiseModel::Unit::Create(3));
|
|
|
|
cout << "Optimizing..." << endl;
|
|
|
|
gttic_(Create_optimizer);
|
|
LevenbergMarquardtOptimizer optimizer(*graph, *initial);
|
|
gttoc_(Create_optimizer);
|
|
tictoc_print_();
|
|
tictoc_reset_();
|
|
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));
|
|
tictoc_reset_();
|
|
|
|
// Compute marginals
|
|
gttic_(ConstructMarginals);
|
|
Marginals marginals(*graph, optimizer.values());
|
|
gttoc_(ConstructMarginals);
|
|
for(Key key: initial->keys()) {
|
|
gttic_(marginalInformation);
|
|
Matrix info = marginals.marginalInformation(key);
|
|
gttoc_(marginalInformation);
|
|
tictoc_finishedIteration_();
|
|
}
|
|
tictoc_print_();
|
|
|
|
} catch(std::exception& e) {
|
|
cout << e.what() << endl;
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|