12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

move things to vSLAM according to new policies

release/4.3a0
Yong-Dian Jian 2010-10-30 05:31:22 +00:00
parent fca73852f1
commit 8c91fe5f53
12 changed files with 16 additions and 502 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/Makefile.am
View File

@ -16,7 +16,6 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial
noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script
noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
noinst_PROGRAMS += Pose2SLAMwCG # Solves a simple Pose2 SLAM example with easy CG solver
noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses

1
examples/Pose2SLAMExample_easy.cpp
View File

@ -22,7 +22,6 @@ int main(int argc, char** argv) {
Key x1(1), x2(2), x3(3);
/* 1. create graph container and add factors to it */
//Graph graph;
Graph graph ;
/* 2.a add prior */

84
examples/Pose2SLAMwCG.cpp
View File

@ -1,84 +0,0 @@
/*
* Pose2SLAMwCG.cpp
*
* Created on: Oct 27, 2010
* Author: Yong-Dian Jian
*/
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/*
* Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
*/
#include <boost/shared_ptr.hpp>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
using namespace std;
using namespace gtsam;
using namespace pose2SLAM;
Graph graph;
Values initial;
Values result;
/* ************************************************************************* */
int main(void) {
/* generate synthetic data */
const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
// create a 3 by 3 grid
// x3 x6 x9
// x2 x5 x8
// x1 x4 x7
graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
graph.addPrior(x1, Pose2(0,0,0), sigma) ;
initial.insert(x1, Pose2( 0, 0, 0));
initial.insert(x2, Pose2( 0, 2.1, 0.01));
initial.insert(x3, Pose2( 0, 3.9,-0.01));
initial.insert(x4, Pose2(2.1,-0.1, 0));
initial.insert(x5, Pose2(1.9, 2.1, 0.02));
initial.insert(x6, Pose2(2.0, 3.9,-0.02));
initial.insert(x7, Pose2(4.0, 0.1, 0.03 ));
initial.insert(x8, Pose2(3.9, 2.1, 0.01));
initial.insert(x9, Pose2(4.1, 3.9,-0.01));
/* done */
graph.print("full graph") ;
initial.print("initial estimate");
result = optimizePCG<Graph,Values, JacobiPreconditioner<GaussianFactorGraph,VectorValues> >(graph, initial);
//result = optimizeCG(graph, initial);
result.print("final result") ;
return 0 ;
}

10
gtsam/linear/ConjugateGradientSolver-inl.h
View File

@ -1,10 +0,0 @@
/*
* ConjugateGradientSolver-inl.h
*
* Created on: Oct 24, 2010
* Author: Yong-Dian Jian
*/
#pragma once
#include <gtsam/linear/ConjugateGradientSolver.h>

135
gtsam/linear/ConjugateGradientSolver.h
View File

@ -1,135 +0,0 @@
/*
* ConjugateGradientSolver.h
*
* Created on: Oct 21, 2010
* Author: Yong-Dian Jian
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
template<class GRAPH, class LINEAR, class VALUES>
class ConjugateGradientSolver : public IterativeSolver {
protected:
typedef boost::shared_ptr<GRAPH> sharedGRAPH ;
typedef boost::shared_ptr<LINEAR> sharedLINEAR ;
typedef boost::shared_ptr<VALUES> sharedVALUES ;
typedef boost::shared_ptr<VectorValues> sharedVectorValues ;
const LINEAR *ptr_;
sharedVectorValues zeros_;
public:
typedef boost::shared_ptr<const ConjugateGradientSolver> shared_ptr ;
// suggested constructor
ConjugateGradientSolver(const GRAPH &graph, const VALUES &initial, const Ordering &ordering, const Parameters &parameters = Parameters()):
IterativeSolver(parameters), ptr_(0), zeros_(boost::make_shared<VectorValues>(initial.zero(ordering))) {}
// to be compatible to NonlinearOptimizer, but shouldn't be called...
ConjugateGradientSolver(const LINEAR &GFG) {
std::cout << "[ConjugateGradientSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
}
// copy constructor
ConjugateGradientSolver(const ConjugateGradientSolver& solver) : IterativeSolver(solver), ptr_(solver.ptr_), zeros_(solver.zeros_){}
shared_ptr update(const LINEAR &graph) const {
return boost::make_shared<ConjugateGradientSolver>(parameters_, &graph, zeros_) ;
}
VectorValues::shared_ptr optimize() const {
VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, *parameters_);
return boost::make_shared<VectorValues>(x) ;
}
// for NonlinearOptimizer update only
ConjugateGradientSolver(const sharedParameters parameters,
const LINEAR *ptr,
const sharedVectorValues zeros):
IterativeSolver(parameters), ptr_(ptr), zeros_(zeros) {}
private:
// shouldn't be used
ConjugateGradientSolver(){}
};
template<class GRAPH, class LINEAR, class VALUES, class PC>
class PreconditionedConjugateGradientSolver : public IterativeSolver {
protected:
typedef boost::shared_ptr<GRAPH> sharedGRAPH ;
typedef boost::shared_ptr<LINEAR> sharedLINEAR ;
typedef boost::shared_ptr<PC> sharedPreconditioner ;
typedef boost::shared_ptr<VALUES> sharedVALUES ;
typedef boost::shared_ptr<VectorValues> sharedVectorValues ;
const LINEAR *ptr_;
sharedPreconditioner pc_ ;
sharedVectorValues zeros_;
public:
typedef boost::shared_ptr<const PreconditionedConjugateGradientSolver> shared_ptr ;
// suggested constructor
PreconditionedConjugateGradientSolver(
const GRAPH &graph,
const VALUES &initial,
const Ordering &ordering,
const Parameters &parameters = Parameters()):
IterativeSolver(parameters), ptr_(0), pc_(), zeros_(boost::make_shared<VectorValues>(initial.zero(ordering))) {}
// to be compatible to NonlinearOptimizer, but shouldn't be called...
PreconditionedConjugateGradientSolver(const LINEAR &GFG) {
std::cout << "[PreconditionedConjugateGradientSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("PreconditionedConjugateGradientSolver: gaussian factor graph initialization not supported");
}
// copy
PreconditionedConjugateGradientSolver(const PreconditionedConjugateGradientSolver& solver) :
IterativeSolver(solver), ptr_(solver.ptr_), pc_(solver.pc_), zeros_(solver.zeros_){}
// called by NonlinearOptimizer
shared_ptr update(const LINEAR &graph) const {
return boost::make_shared<PreconditionedConjugateGradientSolver>(
parameters_,
&graph,
boost::make_shared<PC>(&graph, parameters_, zeros_),
zeros_) ;
}
// optimize with preconditioned conjugate gradient
VectorValues::shared_ptr optimize() const {
VectorValues x = preconditionedConjugateGradientDescent<LINEAR, PC, VectorValues>
(*ptr_, *pc_, *zeros_, *parameters_);
return boost::make_shared<VectorValues>(x) ;
}
PreconditionedConjugateGradientSolver(
const sharedParameters parameters,
const LINEAR *ptr,
const sharedPreconditioner pc,
const sharedVectorValues zeros):
IterativeSolver(parameters), ptr_(ptr), pc_(pc), zeros_(zeros) {}
private:
// shouldn't be used
PreconditionedConjugateGradientSolver(){}
};
} // nsamespace gtsam

2
gtsam/linear/Makefile.am
View File

@ -36,7 +36,7 @@ check_PROGRAMS += tests/testGaussianJunctionTree
# Iterative Methods
headers += iterative-inl.h
sources += iterative.cpp SubgraphPreconditioner.cpp
headers += IterativeOptimizationParameters.h IterativeSolver.h ConjugateGradientSolver.h ConjugateGradientSolver-inl.h
headers += IterativeSolver.h IterativeOptimizationParameters.h
headers += SubgraphSolver.h SubgraphSolver-inl.h
#sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp
#check_PROGRAMS += tests/testBayesNetPreconditioner

88
gtsam/linear/Preconditioner.h
View File

@ -1,88 +0,0 @@
/*
* Preconditioner.h
*
* Created on: Oct 27, 2010
* Author: Yong-Dian Jian
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/IterativeOptimizationParameters.h>
namespace gtsam {
template <class LINEAR, class VALUES>
class Preconditioner {
public:
typedef IterativeOptimizationParameters Parameters;
typedef boost::shared_ptr<Parameters> sharedParameters;
typedef boost::shared_ptr<LINEAR> sharedLinear;
typedef boost::shared_ptr<VALUES> sharedValues ;
public:
Preconditioner(const LINEAR *ptr, const sharedParameters parameters, const sharedValues zeros):
pLinear_(ptr), parameters_(parameters), zeros_(zeros){}
void solve(const VALUES &values, VALUES &result) const { result = values ; }
void solveTranspose(const VALUES &values, VALUES &result) const { result = values ; }
protected:
const LINEAR* pLinear_ ;
const sharedParameters parameters_ ;
const sharedValues zeros_;
private:
Preconditioner(){}
};
template <class LINEAR, class VALUES>
class JacobiPreconditioner : public Preconditioner<LINEAR,VALUES> {
public:
typedef Preconditioner<LINEAR,VALUES> Base ;
typedef IterativeOptimizationParameters Parameters;
typedef boost::shared_ptr<Parameters> sharedParameters;
typedef boost::shared_ptr<LINEAR> sharedLinear;
typedef boost::shared_ptr<VALUES> sharedValues ;
JacobiPreconditioner(const LINEAR *ptr, const sharedParameters parameters, const sharedValues zeros):
Base(ptr, parameters, zeros), scale_(new VALUES(*zeros)) {
initialize() ;
}
void initialize() {
Base::pLinear_->getDiagonalOfHessian(*scale_);
sqrt(*scale_) ;
// check if any zero in scale_
if ( anyZero(*scale_) ) {
std::cout << "[JacobiPreconditioner] some diagonal element of hessian is zero" << std::endl ;
throw std::runtime_error("JacobiPreconditioner");
}
}
void solve(const VALUES &values, VALUES &result) const {
//std::cout << "solve" << std::endl;
ediv(values, *scale_, result) ;
}
void solveTranspose(const VALUES &values, VALUES &result) const {
//std::cout << "solveTranspose" << std::endl;
ediv(values, *scale_, result) ;
}
protected:
sharedValues scale_ ;
private:
JacobiPreconditioner(){}
};
}

117
gtsam/linear/iterative-inl.h
View File

@ -19,11 +19,8 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/IterativeOptimizationParameters.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/iterative.h>
using namespace std;
@ -35,7 +32,7 @@ namespace gtsam {
template<class S, class V, class E>
struct CGState {
typedef IterativeSolver::Parameters Parameters;
typedef IterativeOptimizationParameters Parameters;
const Parameters &parameters_;
int k;
@ -127,7 +124,7 @@ namespace gtsam {
V conjugateGradients(
const S& Ab,
V x,
const IterativeSolver::Parameters &parameters,
const IterativeOptimizationParameters &parameters,
bool steepest = false) {
CGState<S, V, E> state(Ab, x, parameters, steepest);
@ -148,116 +145,6 @@ namespace gtsam {
while (!state.step(Ab, x)) {}
return x;
}
/* ************************************************************************* */
// state for PCG method
template<class LINEAR, class PC, class V>
class PCGState {
public:
typedef IterativeSolver::Parameters Parameters;
typedef V Values;
typedef boost::shared_ptr<Values> sharedValues;
protected:
const LINEAR &Ab_;
const PC &pc_ ;
V x_ ;
const Parameters &parameters_;
bool steepest_;
public:
/* ************************************************************************* */
// Constructor
PCGState(const LINEAR& Ab, const PC &pc, const V &x0, const Parameters &parameters, bool steep):
Ab_(Ab), pc_(pc), x_(x0), parameters_(parameters),steepest_(steep) {}
V run() {
// refer to Bjorck pp. 294
V r = Ab_.allocateVectorValuesb() ;
V q = V::SameStructure(r) ;
V p = V::SameStructure(x_) ;
V s = V::SameStructure(x_) ;
V t = V::SameStructure(x_) ;
V tmp = V::SameStructure(x_) ;
// initial PCG
Ab_.residual(x_, r) ;
Ab_.transposeMultiply(r, tmp) ;
pc_.solveTranspose(tmp, s) ;
p = s ;
double gamma = dot(s,s), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ;
const double threshold =
::max(parameters_.epsilon_abs(),
parameters_.epsilon() * parameters_.epsilon() * gamma);
const int iMaxIterations = parameters_.maxIterations();
const int iReset = parameters_.reset() ;
if (parameters_.verbosity())
cout << "PCG: epsilon = " << parameters_.epsilon()
<< ", maxIterations = " << parameters_.maxIterations()
<< ", ||g0||^2 = " << gamma
<< ", threshold = " << threshold << endl;
for ( int k = 0 ; k < iMaxIterations ; ++k ) {
if ( gamma < threshold ) break ;
if ( k % iReset == 0) {
Ab_.residual(x_, r) ;
Ab_.transposeMultiply(r, tmp) ;
pc_.solveTranspose(tmp, s) ;
p = s ;
gamma = dot(s,s) ;
}
pc_.solve(p, t) ;
Ab_.multiply(t, q) ;
alpha = gamma / dot(q,q) ;
axpy( alpha, t, x_) ;
axpy(-alpha, q, r) ;
Ab_.transposeMultiply(r, tmp) ;
pc_.solveTranspose(tmp, s) ;
new_gamma = dot(s,s) ;
if (parameters_.verbosity())
cout << "iteration " << k
<< ": alpha = " << alpha
<< ", dotg = " << new_gamma << endl;
beta = new_gamma / gamma ;
scal(beta,p) ;
axpy(1.0,s,p) ;
gamma = new_gamma ;
}
return x_ ;
} // function
private:
PCGState(){}
}; // PCGState Class
/* ************************************************************************* */
template<class LINEAR, class PC, class V>
V preconditionedConjugateGradientDescent(
const LINEAR& Ab,
const PC& pc,
V x,
const IterativeSolver::Parameters &parameters,
bool steepest = false) {
PCGState<LINEAR, PC, V> state(Ab, pc, x, parameters, steepest);
return state.run() ;
}
/* ************************************************************************* */
} // namespace gtsam

14
gtsam/linear/iterative.cpp
View File

@ -20,8 +20,6 @@
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/IterativeOptimizationParameters.h>
#include <gtsam/linear/iterative-inl.h>
@ -38,31 +36,31 @@ namespace gtsam {
/* ************************************************************************* */
Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::Parameters & parameters) {
Vector steepestDescent(const System& Ab, const Vector& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
}
Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::Parameters & parameters) {
Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
}
/* ************************************************************************* */
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::Parameters & parameters) {
Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeOptimizationParameters & parameters) {
System Ab(A, b);
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
}
Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::Parameters & parameters) {
Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeOptimizationParameters & parameters) {
System Ab(A, b);
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
}
/* ************************************************************************* */
VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::Parameters & parameters) {
VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters, true);
}
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::Parameters & parameters) {
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters);
}

15
gtsam/linear/iterative.h
View File

@ -20,8 +20,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/IterativeOptimizationParameters.h>
namespace gtsam {
@ -92,7 +91,7 @@ namespace gtsam {
Vector steepestDescent(
const System& Ab,
const Vector& x,
const IterativeSolver::Parameters & parameters);
const IterativeOptimizationParameters & parameters);
/**
* Method of conjugate gradients (CG), System version
@ -100,7 +99,7 @@ namespace gtsam {
Vector conjugateGradientDescent(
const System& Ab,
const Vector& x,
const IterativeSolver::Parameters & parameters);
const IterativeOptimizationParameters & parameters);
/** convenience calls using matrices, will create System class internally: */
@ -111,7 +110,7 @@ namespace gtsam {
const Matrix& A,
const Vector& b,
const Vector& x,
const IterativeSolver::Parameters & parameters);
const IterativeOptimizationParameters & parameters);
/**
* Method of conjugate gradients (CG), Matrix version
@ -120,7 +119,7 @@ namespace gtsam {
const Matrix& A,
const Vector& b,
const Vector& x,
const IterativeSolver::Parameters & parameters);
const IterativeOptimizationParameters & parameters);
class GaussianFactorGraph;
@ -130,7 +129,7 @@ namespace gtsam {
VectorValues steepestDescent(
const GaussianFactorGraph& fg,
const VectorValues& x,
const IterativeSolver::Parameters & parameters);
const IterativeOptimizationParameters & parameters);
/**
* Method of conjugate gradients (CG), Gaussian Factor Graph version
@ -138,7 +137,7 @@ namespace gtsam {
VectorValues conjugateGradientDescent(
const GaussianFactorGraph& fg,
const VectorValues& x,
const IterativeSolver::Parameters & parameters);
const IterativeOptimizationParameters & parameters);
} // namespace gtsam

49
gtsam/nonlinear/NonlinearOptimization-inl.h
View File

@ -23,7 +23,6 @@
#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>
@ -70,52 +69,6 @@ namespace gtsam {
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, 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();
}
template<class G, class T, class PC>
T optimizePCG(const G& graph,
const T& initialEstimate,
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
typedef PreconditionedConjugateGradientSolver<G,GaussianFactorGraph,T,PC> 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, 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
*/
@ -149,8 +102,6 @@ namespace gtsam {
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");
}

2
gtsam/nonlinear/NonlinearOptimization.h
View File

@ -37,11 +37,9 @@ namespace gtsam {
enum LinearSolver{
SEQUENTIAL, // Sequential elimination
MULTIFRONTAL, // Multi-frontal elimination
CG, // vanilla conjugate gradient
SPCG, // Subgraph Preconditioned Conjugate Gradient
};
/**
* optimization that returns the values
*/