add a simple spcg implementation and revive the example
Yong-Dian Jian
parent
a75c9f1da3
commit
7d132ef217
|
|
@ -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
|
||||
|
|
@ -9,79 +9,60 @@
|
|||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file Pose2SLAMwSPCG_easy.cpp
|
||||
* @brief Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
|
||||
* @author Yong Dian
|
||||
* Created October 21, 2010
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#if ENABLE_SPCG
|
||||
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace pose2SLAM;
|
||||
|
||||
Graph graph;
|
||||
Values initial, 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);
|
||||
// 1. Create graph container and add factors to it
|
||||
pose2SLAM::Graph graph ;
|
||||
|
||||
// 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) ;
|
||||
// 2a. Add Gaussian prior
|
||||
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
|
||||
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1));
|
||||
graph.addPrior(1, priorMean, priorNoise);
|
||||
|
||||
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 */
|
||||
// 2b. Add odometry factors
|
||||
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1));
|
||||
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
|
||||
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
|
||||
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
|
||||
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
|
||||
|
||||
// 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 ;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
int main() {
|
||||
std::cout << "SPCG is currently disabled" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -96,7 +96,7 @@ struct PreconditionerParameters {
|
|||
|
||||
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)
|
||||
: kernel_(kernel), type_(Combinatorial), combinatorial_(combinatorial), verbosity_(verbosity) {}
|
||||
|
||||
|
|
@ -180,13 +180,13 @@ public:
|
|||
|
||||
PreconditionerParameters preconditioner_;
|
||||
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 */
|
||||
|
||||
public:
|
||||
|
||||
IterativeOptimizationParameters()
|
||||
: preconditioner_(), cg_(), kernel_(PCG), verbosity_(SILENT) {}
|
||||
: preconditioner_(), cg_(), kernel_(LSPCG), verbosity_(SILENT) {}
|
||||
|
||||
IterativeOptimizationParameters(const IterativeOptimizationParameters &p) :
|
||||
preconditioner_(p.preconditioner_), cg_(p.cg_), kernel_(p.kernel_), verbosity_(p.verbosity_) {}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -46,7 +39,7 @@ public:
|
|||
|
||||
virtual VectorValues::shared_ptr optimize () = 0;
|
||||
|
||||
Parameters::shared_ptr parameters() { return parameters_ ; }
|
||||
inline Parameters::shared_ptr parameters() { return parameters_ ; }
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 ¶meters)
|
||||
: 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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -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 ¶meters);
|
||||
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);
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -21,6 +21,7 @@
|
|||
#include <gtsam/base/cholesky.h> // For NegativeMatrixException
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
#include <gtsam/linear/SimpleSPCGSolver.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -71,7 +72,8 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction()));
|
||||
}
|
||||
else if ( params_.isCG() ) {
|
||||
throw runtime_error("todo: ");
|
||||
SimpleSPCGSolver solver(dampedSystem, *params_.iterativeParams);
|
||||
delta = *solver.optimize();
|
||||
}
|
||||
else {
|
||||
throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");
|
||||
|
|
|
|||
Loading…
Reference in New Issue