Lago timing
parent
e19f3c5902
commit
8fe24183eb
|
@ -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>
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -1 +1 @@
|
|||
gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam")
|
||||
gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam")
|
||||
|
|
|
@ -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;
|
||||
}
|
Loading…
Reference in New Issue