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

add a simple spcg implementation and revive the example

release/4.3a0
Yong-Dian Jian 2012-06-03 14:52:26 +00:00
parent a75c9f1da3
commit 7d132ef217
7 changed files with 366 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
examples/Pose2SLAMwSPCG_advanced.cpp
View File

@ -1,109 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMwSPCG_advanced.cpp
* @brief Solve a simple 3 by 3 grid of Pose2 SLAM problem by using advanced SPCG interface
* @author Yong Dian
* Created October 21, 2010
*/
#include <iostream>
#if ENABLE_SPCG
#include <boost/shared_ptr.hpp>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
using namespace std;
using namespace gtsam;
using namespace pose2SLAM;
typedef boost::shared_ptr<Graph> sharedGraph ;
typedef boost::shared_ptr<Values> sharedValue ;
//typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
typedef boost::shared_ptr<Solver> sharedSolver ;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer;
sharedGraph graph;
sharedValue initial;
Values result;
/* ************************************************************************* */
int main(void) {
/* generate synthetic data */
const SharedNoiseModel 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);
graph = boost::make_shared<Graph>() ;
initial = boost::make_shared<Values>() ;
// 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 with generating data */
graph->print("full graph") ;
initial->print("initial estimate") ;
sharedSolver solver(new Solver(*graph, *initial)) ;
SPCGOptimizer optimizer(graph, initial, solver->ordering(), solver) ;
cout << "before optimization, sum of error is " << optimizer.error() << endl;
SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt();
cout << "after optimization, sum of error is " << optimizer2.error() << endl;
result = *optimizer2.values() ;
result.print("final result") ;
return 0 ;
}
#else
int main() {
std::cout << "SPCG is currently disabled" << std::endl;
return 0;
}
#endif

101
examples/Pose2SLAMwSPCG_easy.cpp
View File

@ -9,79 +9,60 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/* #include <gtsam/linear/IterativeOptimizationParameters.h>
* @file Pose2SLAMwSPCG_easy.cpp #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
* @brief Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface #include <gtsam/slam/pose2SLAM.h>
* @author Yong Dian
* Created October 21, 2010
*/
#include <iostream>
#if ENABLE_SPCG
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace pose2SLAM;
Graph graph;
Values initial, result;
/* ************************************************************************* */ /* ************************************************************************* */
int main(void) { int main(void) {
/* generate synthetic data */ // 1. Create graph container and add factors to it
const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1)); pose2SLAM::Graph graph ;
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 // 2a. Add Gaussian prior
// x3 x6 x9 Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
// x2 x5 x8 SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1));
// x1 x4 x7 graph.addPrior(1, priorMean, priorNoise);
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)); // 2b. Add odometry factors
initial.insert(x2, Pose2( 0, 2.1, 0.01)); SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1));
initial.insert(x3, Pose2( 0, 3.9,-0.01)); graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
initial.insert(x4, Pose2(2.1,-0.1, 0)); graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
initial.insert(x5, Pose2(1.9, 2.1, 0.02)); graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
initial.insert(x6, Pose2(2.0, 3.9,-0.02)); graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
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 */
// 2c. Add pose constraint
SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
// print
graph.print("\nFactor graph:\n");
// 3. Create the data structure to hold the initialEstimate estinmate to the solution
pose2SLAM::Values initialEstimate;
Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3);
Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4);
Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5);
initialEstimate.print("\nInitial estimate:\n ");
cout << "initial error = " << graph.error(initialEstimate) << endl ;
// 4. Single Step Optimization using Levenberg-Marquardt
LevenbergMarquardtParams param;
param.linearSolverType = SuccessiveLinearizationParams::CG;
param.iterativeParams = boost::make_shared<IterativeOptimizationParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param);
Values result = optimizer.optimize();
cout << "final error = " << graph.error(result) << endl;
graph.print("full graph") ;
initial.print("initial estimate");
result = optimizeSPCG(graph, initial);
result.print("final result") ;
return 0 ; return 0 ;
} }
#else
int main() {
std::cout << "SPCG is currently disabled" << std::endl;
return 0;
}
#endif

6
gtsam/linear/IterativeOptimizationParameters.h
View File

@ -96,7 +96,7 @@ struct PreconditionerParameters {
enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */ enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */
PreconditionerParameters(): kernel_(CHOLMOD), type_(Combinatorial), verbosity_(SILENT) {} PreconditionerParameters(): kernel_(GTSAM), type_(Combinatorial), verbosity_(SILENT) {}
PreconditionerParameters(Kernel kernel, const CombinatorialParameters &combinatorial, Verbosity verbosity) PreconditionerParameters(Kernel kernel, const CombinatorialParameters &combinatorial, Verbosity verbosity)
: kernel_(kernel), type_(Combinatorial), combinatorial_(combinatorial), verbosity_(verbosity) {} : kernel_(kernel), type_(Combinatorial), combinatorial_(combinatorial), verbosity_(verbosity) {}
@ -180,13 +180,13 @@ public:
PreconditionerParameters preconditioner_; PreconditionerParameters preconditioner_;
ConjugateGradientParameters cg_; ConjugateGradientParameters cg_;
enum Kernel { PCG = 0 /*, PCGPlus*/, LSPCG } kernel_ ; /* Iterative Method Kernel */ enum Kernel { PCG = 0, LSPCG } kernel_ ; /* Iterative Method Kernel */
enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */ enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */
public: public:
IterativeOptimizationParameters() IterativeOptimizationParameters()
: preconditioner_(), cg_(), kernel_(PCG), verbosity_(SILENT) {} : preconditioner_(), cg_(), kernel_(LSPCG), verbosity_(SILENT) {}
IterativeOptimizationParameters(const IterativeOptimizationParameters &p) : IterativeOptimizationParameters(const IterativeOptimizationParameters &p) :
preconditioner_(p.preconditioner_), cg_(p.cg_), kernel_(p.kernel_), verbosity_(p.verbosity_) {} preconditioner_(p.preconditioner_), cg_(p.cg_), kernel_(p.kernel_), verbosity_(p.verbosity_) {}

11
gtsam/linear/IterativeSolver.h
View File

@ -9,18 +9,11 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/**
* @file IterativeSolver.h
* @date Oct 24, 2010
* @author Yong-Dian Jian
* @brief Base Class for all iterative solvers of linear systems
*/
#pragma once #pragma once
#include <boost/shared_ptr.hpp>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/IterativeOptimizationParameters.h> #include <gtsam/linear/IterativeOptimizationParameters.h>
#include <boost/shared_ptr.hpp>
namespace gtsam { namespace gtsam {
@ -46,7 +39,7 @@ public:
virtual VectorValues::shared_ptr optimize () = 0; virtual VectorValues::shared_ptr optimize () = 0;
Parameters::shared_ptr parameters() { return parameters_ ; } inline Parameters::shared_ptr parameters() { return parameters_ ; }
}; };
} }

230
gtsam/linear/SimpleSPCGSolver.cpp Normal file
View File

@ -0,0 +1,230 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#include <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/EliminationTree.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
namespace gtsam {
/* utility function */
std::vector<size_t> extractRowSpec_(const FactorGraph<JacobianFactor>& jfg) {
std::vector<size_t> spec; spec.reserve(jfg.size());
BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) {
spec.push_back(jf->rows());
}
return spec;
}
std::vector<size_t> extractColSpec_(const FactorGraph<GaussianFactor>& gfg, const VariableIndex &index) {
const size_t n = index.size();
std::vector<size_t> spec(n, 0);
for ( Index i = 0 ; i < n ; ++i ) {
const GaussianFactor &gf = *(gfg[index[i].front()]);
for ( GaussianFactor::const_iterator it = gf.begin() ; it != gf.end() ; ++it ) {
spec[*it] = gf.getDim(it);
}
}
return spec;
}
SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr &parameters)
: Base(parameters)
{
std::vector<size_t> colSpec = extractColSpec_(gfg, VariableIndex(gfg));
nVar_ = colSpec.size();
/* split the factor graph into At (tree) and Ac (constraints) */
GaussianFactorGraph::shared_ptr At;
boost::tie(At, Ac_) = this->splitGraph(gfg);
/* construct row vector spec of the new system */
nAc_ = Ac_->size();
std::vector<size_t> rowSpec = extractRowSpec_(*Ac_);
for ( Index i = 0 ; i < nVar_ ; ++i ) {
rowSpec.push_back(colSpec[i]);
}
/* solve the tree with direct solver. get preconditioner */
Rt_ = EliminationTree<GaussianFactor>::Create(*At)->eliminate(EliminateQR);
xt_ = boost::make_shared<VectorValues>(gtsam::optimize(*Rt_));
/* initial value for the lspcg method */
y0_ = boost::make_shared<VectorValues>(VectorValues::Zero(colSpec));
/* the right hand side of the new system */
by_ = boost::make_shared<VectorValues>(VectorValues::Zero(rowSpec));
for ( Index i = 0 ; i < nAc_ ; ++i ) {
JacobianFactor::shared_ptr jf = (*Ac_)[i];
Vector xi = internal::extractVectorValuesSlices(*xt_, jf->begin(), jf->end());
(*by_)[i] = jf->getb() - jf->getA()*xi;
}
/* allocate buffer for row and column vectors */
tmpY_ = boost::make_shared<VectorValues>(VectorValues::Zero(colSpec));
tmpB_ = boost::make_shared<VectorValues>(VectorValues::Zero(rowSpec));
}
/* implements the CGLS method in Section 7.4 of Bjork's book */
VectorValues::shared_ptr SimpleSPCGSolver::optimize (const VectorValues &initial) {
VectorValues::shared_ptr x(new VectorValues(initial));
VectorValues r = VectorValues::Zero(*by_),
q = VectorValues::Zero(*by_),
p = VectorValues::Zero(*y0_),
s = VectorValues::Zero(*y0_);
residual(*x, r);
transposeMultiply(r, s) ;
p.vector() = s.vector() ;
double gamma = s.vector().squaredNorm(), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ;
const double threshold =
::max(parameters_->epsilon_abs(),
parameters_->epsilon() * parameters_->epsilon() * gamma);
const size_t iMaxIterations = parameters_->maxIterations();
if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR )
cout << "[SimpleSPCGSolver] epsilon = " << parameters_->epsilon()
<< ", max = " << parameters_->maxIterations()
<< ", ||r0|| = " << std::sqrt(gamma)
<< ", threshold = " << threshold << std::endl;
size_t k ;
for ( k = 1 ; k < iMaxIterations ; ++k ) {
multiply(p, q);
alpha = gamma / q.vector().squaredNorm() ;
x->vector() += (alpha * p.vector());
r.vector() -= (alpha * q.vector());
transposeMultiply(r, s);
new_gamma = s.vector().squaredNorm();
beta = new_gamma / gamma ;
p.vector() = s.vector() + beta * p.vector();
gamma = new_gamma ;
if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR) {
cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl;
}
if ( gamma < threshold ) break ;
} // k
if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR )
cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl;
/* transform y back to x */
return this->transform(*x);
}
void SimpleSPCGSolver::residual(const VectorValues &input, VectorValues &output) {
output.vector() = by_->vector();
this->multiply(input, *tmpB_);
axpy(-1.0, *tmpB_, output);
}
void SimpleSPCGSolver::multiply(const VectorValues &input, VectorValues &output) {
this->backSubstitute(input, *tmpY_);
gtsam::multiply(*Ac_, *tmpY_, output);
for ( Index i = 0 ; i < nVar_ ; ++i ) {
output[i + nAc_] = input[i];
}
}
void SimpleSPCGSolver::transposeMultiply(const VectorValues &input, VectorValues &output) {
gtsam::transposeMultiply(*Ac_, input, *tmpY_);
this->transposeBackSubstitute(*tmpY_, output);
for ( Index i = 0 ; i < nVar_ ; ++i ) {
output[i] += input[nAc_+i];
}
}
void SimpleSPCGSolver::backSubstitute(const VectorValues &input, VectorValues &output) {
for ( Index i = 0 ; i < input.size() ; ++i ) {
output[i] = input[i] ;
}
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, *Rt_) {
const Index key = *(cg->beginFrontals());
Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents());
xS = input[key] - cg->get_S() * xS;
output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS);
}
}
void SimpleSPCGSolver::transposeBackSubstitute(const VectorValues &input, VectorValues &output) {
for ( Index i = 0 ; i < input.size() ; ++i ) {
output[i] = input[i] ;
}
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, *Rt_) {
const Index key = *(cg->beginFrontals());
output[key] = gtsam::backSubstituteUpper(output[key], Matrix(cg->get_R()));
const Vector d = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents())
- cg->get_S().transpose() * output[key];
internal::writeVectorValuesSlices(d, output, cg->beginParents(), cg->endParents());
}
}
VectorValues::shared_ptr SimpleSPCGSolver::transform(const VectorValues &y) {
VectorValues::shared_ptr x = boost::make_shared<VectorValues>(VectorValues::Zero(y));
this->backSubstitute(y, *x);
axpy(1.0, *xt_, *x);
return x;
}
boost::tuple<GaussianFactorGraph::shared_ptr, FactorGraph<JacobianFactor>::shared_ptr>
SimpleSPCGSolver::splitGraph(const GaussianFactorGraph &gfg) {
VariableIndex index(gfg);
size_t n = index.size();
std::vector<bool> connected(n, false);
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
FactorGraph<JacobianFactor>::shared_ptr Ac( new FactorGraph<JacobianFactor>());
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
bool augment = false ;
/* check whether this factor should be augmented to the "tree" graph */
if ( gf->keys().size() == 1 ) augment = true;
else {
BOOST_FOREACH ( const Index key, *gf ) {
if ( connected[key] == false ) {
augment = true ;
}
connected[key] = true;
}
}
if ( augment ) At->push_back(gf);
else Ac->push_back(boost::dynamic_pointer_cast<JacobianFactor>(gf));
}
// gfg.print("gfg");
// At->print("At");
// Ac->print("Ac");
return boost::tie(At, Ac);
}
}

87
gtsam/linear/SimpleSPCGSolver.h Normal file
View File

@ -0,0 +1,87 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
namespace gtsam {
/**
* This class gives a simple implementation to the SPCG solver presented in Dellaert et al. IROS'10.
* Given a linear least-squares problem f(x) = |A x - b|^2.
* We split the problem into f(x) = |At - bt|^2 + |Ac - bc|^2 where At denotes the "tree" part,
* and Ac denotes the "constraint" part. At is factorized into Qt Rt, and we compute ct = Qt\bt, and xt = Rt\ct.
* Then we solve reparametrized problem f(y) = |y|^2 + |Ac Rt \ y - by|^2, where y = Rt(x - xt), and by = (bc - Ac xt)
* In the matrix form, it is equivalent to solving [Ac Rt^{-1} ; I ] y = [by ; 0].
*/
class SimpleSPCGSolver : public IterativeSolver {
public:
typedef IterativeSolver Base;
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
protected:
size_t nVar_ ; /* number of variables */
size_t nAc_ ; /* number of factors in Ac */
/* for SPCG */
FactorGraph<JacobianFactor>::shared_ptr Ac_;
GaussianBayesNet::shared_ptr Rt_;
VectorValues::shared_ptr xt_;
VectorValues::shared_ptr y0_;
VectorValues::shared_ptr by_; /* [bc_bar ; 0 ] */
/* buffer for row and column vectors in */
VectorValues::shared_ptr tmpY_;
VectorValues::shared_ptr tmpB_;
public:
SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr &parameters);
virtual ~SimpleSPCGSolver() {}
virtual VectorValues::shared_ptr optimize () {return optimize(*y0_);}
protected:
VectorValues::shared_ptr optimize (const VectorValues &initial);
/* output = [bc_bar ; 0 ] - [Ac Rt^{-1} ; I] y */
void residual(const VectorValues &input, VectorValues &output);
/* output = [Ac Rt^{-1} ; I] input */
void multiply(const VectorValues &input, VectorValues &output);
/* output = [Rt^{-T} Ac^T, I] input */
void transposeMultiply(const VectorValues &input, VectorValues &output);
/* output = Rt^{-1} input */
void backSubstitute(const VectorValues &rhs, VectorValues &sol) ;
/* output = Rt^{-T} input */
void transposeBackSubstitute(const VectorValues &rhs, VectorValues &sol) ;
/* return x = Rt^{-1} y + x_t */
VectorValues::shared_ptr transform(const VectorValues &y);
/* naively split a gaussian factor graph into tree and constraint parts */
boost::tuple<GaussianFactorGraph::shared_ptr, FactorGraph<JacobianFactor>::shared_ptr>
splitGraph(const GaussianFactorGraph &gfg);
};
}

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/base/cholesky.h> // For NegativeMatrixException #include <gtsam/base/cholesky.h> // For NegativeMatrixException
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/SimpleSPCGSolver.h>
using namespace std; using namespace std;
@ -71,7 +72,8 @@ void LevenbergMarquardtOptimizer::iterate() {
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction())); delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction()));
} }
else if ( params_.isCG() ) { else if ( params_.isCG() ) {
throw runtime_error("todo: "); SimpleSPCGSolver solver(dampedSystem, *params_.iterativeParams);
delta = *solver.optimize();
} }
else { else {
throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination"); throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");