Lago timing

release/4.3a0
dellaert 2014-05-31 14:37:29 -04:00
parent e19f3c5902
commit 8fe24183eb
4 changed files with 85 additions and 1 deletions

View File

@ -2857,6 +2857,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeLago.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>

View File

@ -21,6 +21,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/timing.h>
#include <boost/math/special_functions.hpp>
@ -198,6 +199,7 @@ GaussianFactorGraph buildLinearOrientationGraph(
/* ************************************************************************* */
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
gttic_(buildPose2graph);
NonlinearFactorGraph pose2Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
@ -250,6 +252,7 @@ static PredecessorMap<Key> findOdometricPath(
// Return the orientations of a graph including only BetweenFactors<Pose2>
static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
bool useOdometricPath) {
gttic_(computeOrientations);
// Find a minimum spanning tree
PredecessorMap<Key> tree;
@ -293,6 +296,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
/* ************************************************************************* */
Values computePoses(const NonlinearFactorGraph& pose2graph,
VectorValues& orientationsLago) {
gttic_(computePoses);
// Linearized graph on full poses
GaussianFactorGraph linearPose2graph;

View File

@ -1 +1 @@
gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam")
gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam")

View File

@ -0,0 +1,72 @@
/* ----------------------------------------------------------------------------
* 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 timeVirtual.cpp
* @brief Time the overhead of using virtual destructors and methods
* @author Richard Roberts
* @date Dec 3, 2010
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/lago.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/base/timing.h>
#include <iostream>
using namespace std;
using namespace gtsam;
int main(int argc, char *argv[]) {
size_t trials = 1000;
// read graph
NonlinearFactorGraph g;
Values initial;
string inputFile = findExampleDataFile("noisyToyGraph");
readG2o(inputFile, g, initial);
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Sigmas(Vector3(0, 0, 0));
g.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
// LAGO
for (size_t i = 0; i < trials; i++) {
{
gttic_(lago);
gttic_(init);
Values lagoInitial = lago::initialize(g);
gttoc_(init);
gttic_(refine);
GaussNewtonOptimizer optimizer(g, lagoInitial);
Values result = optimizer.optimize();
gttoc_(refine);
}
{
gttic_(optimize);
GaussNewtonOptimizer optimizer(g, initial);
Values result = optimizer.optimize();
}
tictoc_finishedIteration_();
}
tictoc_print_();
return 0;
}