140 lines
4.6 KiB
C++
140 lines
4.6 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/*
|
|
* NonlinearOptimization-inl.h
|
|
*
|
|
* Created on: Oct 17, 2010
|
|
* Author: Kai Ni
|
|
* Description: Easy interfaces for NonlinearOptimizer
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <boost/make_shared.hpp>
|
|
|
|
#include <gtsam/linear/GaussianSequentialSolver.h>
|
|
#include <gtsam/linear/GaussianMultifrontalSolver.h>
|
|
#include <gtsam/linear/ConjugateGradientSolver.h>
|
|
#include <gtsam/linear/SubgraphSolver-inl.h>
|
|
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
|
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* The Elimination solver
|
|
*/
|
|
template<class G, class T>
|
|
T optimizeSequential(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) {
|
|
|
|
// Use a variable ordering from COLAMD
|
|
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
|
|
|
|
// initial optimization state is the same in both cases tested
|
|
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
|
|
Optimizer optimizer(boost::make_shared<const G>(graph),
|
|
boost::make_shared<const T>(initialEstimate), ordering);
|
|
|
|
// Levenberg-Marquardt
|
|
Optimizer result = optimizer.levenbergMarquardt(parameters);
|
|
return *result.values();
|
|
}
|
|
|
|
/**
|
|
* The multifrontal solver
|
|
*/
|
|
template<class G, class T>
|
|
T optimizeMultiFrontal(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) {
|
|
|
|
// Use a variable ordering from COLAMD
|
|
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
|
|
|
|
// initial optimization state is the same in both cases tested
|
|
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
|
Optimizer optimizer(boost::make_shared<const G>(graph),
|
|
boost::make_shared<const T>(initialEstimate), ordering);
|
|
|
|
// Levenberg-Marquardt
|
|
Optimizer result = optimizer.levenbergMarquardt(parameters);
|
|
return *result.values();
|
|
}
|
|
|
|
/**
|
|
* The cg solver
|
|
*/
|
|
template<class G, class T>
|
|
T optimizeCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
|
|
|
|
typedef ConjugateGradientSolver<G,GaussianFactorGraph,T> Solver;
|
|
typedef boost::shared_ptr<Solver> sharedSolver;
|
|
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> Optimizer;
|
|
|
|
Ordering::shared_ptr ordering = initialEstimate.orderingArbitrary() ;
|
|
sharedSolver solver = boost::make_shared<Solver>(graph, initialEstimate, *ordering, boost::make_shared<IterativeOptimizationParameters>());
|
|
Optimizer optimizer(
|
|
boost::make_shared<const G>(graph),
|
|
boost::make_shared<const T>(initialEstimate),
|
|
ordering,
|
|
solver);
|
|
|
|
// Levenberg-Marquardt
|
|
Optimizer result = optimizer.levenbergMarquardt(parameters);
|
|
return *result.values();
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* The sparse preconditioned conjucate gradient solver
|
|
*/
|
|
template<class G, class T>
|
|
T optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
|
|
|
|
// initial optimization state is the same in both cases tested
|
|
typedef SubgraphSolver<G,GaussianFactorGraph,T> Solver;
|
|
typedef boost::shared_ptr<Solver> shared_Solver;
|
|
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer;
|
|
shared_Solver solver = boost::make_shared<Solver>(graph, initialEstimate);
|
|
SPCGOptimizer optimizer(
|
|
boost::make_shared<const G>(graph),
|
|
boost::make_shared<const T>(initialEstimate),
|
|
solver->ordering(),
|
|
solver);
|
|
|
|
// Levenberg-Marquardt
|
|
SPCGOptimizer result = optimizer.levenbergMarquardt(parameters);
|
|
return *result.values();
|
|
}
|
|
|
|
/**
|
|
* optimization that returns the values
|
|
*/
|
|
template<class G, class T>
|
|
T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters,
|
|
const enum LinearSolver& solver) {
|
|
switch (solver) {
|
|
case SEQUENTIAL:
|
|
return optimizeSequential<G,T>(graph, initialEstimate, parameters);
|
|
case MULTIFRONTAL:
|
|
return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters);
|
|
case CG:
|
|
return optimizeCG<G,T>(graph, initialEstimate, parameters);
|
|
case SPCG:
|
|
throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
|
|
}
|
|
throw runtime_error("optimize: undefined solver");
|
|
}
|
|
|
|
} //namespace gtsam
|