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

remove simpleSPCG 

reorg SubgraphSolver
add unit test for SubgraphSolver
release/4.3a0
Yong-Dian Jian 2012-09-03 19:43:08 +00:00
parent 21d9d8aa0c
commit af652b0e04
13 changed files with 316 additions and 496 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/Pose2SLAMwSPCG.cpp
View File

@ -54,13 +54,9 @@
// for each variable, held in a Values container. // for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
// ???
#include <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -110,14 +106,6 @@ int main(int argc, char** argv) {
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
{
parameters.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
cout << "simple spcg solver final error = " << graph.error(result) << endl;
}
{ {
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>(); parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);

15
gtsam/linear/JacobianFactorGraph.cpp
View File

@ -115,7 +115,20 @@ JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gf
return jfg; return jfg;
} }
/* ************************************************************************* */
JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg) {
JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph());
jfg->reserve(gfg.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr & factor, gfg) {
if( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor) ) {
jfg->push_back(jf);
}
else {
jfg->push_back(boost::make_shared<JacobianFactor>(*factor));
}
}
return jfg;
}
} // namespace } // namespace

4
gtsam/linear/JacobianFactorGraph.h
View File

@ -67,7 +67,9 @@ namespace gtsam {
void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x); void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x);
/** dynamic_cast the gaussian factors down to jacobian factors */ /** dynamic_cast the gaussian factors down to jacobian factors, may throw exception if it contains non-Jacobian Factor */
JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg); JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg);
/** convert the gaussian factors down to jacobian factors, may duplicate factors if it contains Hessian Factor */
JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg);
} }

226
gtsam/linear/SimpleSPCGSolver.cpp
View File

@ -1,226 +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
* -------------------------------------------------------------------------- */
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/SimpleSPCGSolver.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 &parameters)
: Base(), parameters_(parameters)
{
std::vector<size_t> colSpec = extractColSpec_(gfg, VariableIndex(gfg));
nVar_ = colSpec.size();
/* Split the factor graph into At (tree) and Ac (constraints)
* Note: This part has to be refined for your graph/application */
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 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 = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
const size_t iMaxIterations = parameters_.maxIterations();
const Parameters::Verbosity verbosity = parameters_.verbosity();
if ( verbosity >= ConjugateGradientParameters::ERROR )
std::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 ( verbosity >= ConjugateGradientParameters::ERROR) {
std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::endl;
}
if ( gamma < threshold ) break ;
} // k
if ( verbosity >= ConjugateGradientParameters::ERROR )
std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::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 SimpleSPCGSolver::transform(const VectorValues &y) {
VectorValues x = 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));
}
return boost::tie(At, Ac);
}
}

98
gtsam/linear/SimpleSPCGSolver.h
View File

@ -1,98 +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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/JacobianFactor.h>
namespace gtsam {
struct SimpleSPCGSolverParameters : public ConjugateGradientParameters {
typedef ConjugateGradientParameters Base;
SimpleSPCGSolverParameters() : Base() {}
};
/**
* This class gives a simple implementation to the SPCG solver presented in Dellaert et al in IROS'10.
*
* Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into
* \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
* \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
* Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
*
* In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it
* with the least-squares variation of the conjugate gradient method.
*
* Note: A full SPCG implementation will come up soon in the next release.
*
* \nosubgrouping
*/
class SimpleSPCGSolver : public IterativeSolver {
public:
typedef IterativeSolver Base;
typedef SimpleSPCGSolverParameters Parameters;
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
protected:
size_t nVar_ ; ///< number of variables \f$ x \f$
size_t nAc_ ; ///< number of factors in \f$ A_c \f$
FactorGraph<JacobianFactor>::shared_ptr Ac_; ///< the constrained part of the graph
GaussianBayesNet::shared_ptr Rt_; ///< the gaussian bayes net of the tree part of the graph
VectorValues::shared_ptr xt_; ///< the solution of the \f$ A_t^{-1} b_t \f$
VectorValues::shared_ptr y0_; ///< a column zero vector
VectorValues::shared_ptr by_; ///< \f$ [\bar{b_y} ; 0 ] \f$
VectorValues::shared_ptr tmpY_; ///< buffer for the column vectors
VectorValues::shared_ptr tmpB_; ///< buffer for the row vectors
Parameters parameters_; ///< Parameters for iterative method
public:
SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters &parameters);
virtual ~SimpleSPCGSolver() {}
virtual VectorValues optimize () {return optimize(*y0_);}
protected:
VectorValues optimize (const VectorValues &initial);
/** output = \f$ [\bar{b_y} ; 0 ] - [A_c R_t^{-1} ; I] \f$ input */
void residual(const VectorValues &input, VectorValues &output);
/** output = \f$ [A_c R_t^{-1} ; I] \f$ input */
void multiply(const VectorValues &input, VectorValues &output);
/** output = \f$ [R_t^{-T} A_c^T, I] \f$ input */
void transposeMultiply(const VectorValues &input, VectorValues &output);
/** output = \f$ R_t^{-1} \f$ input */
void backSubstitute(const VectorValues &rhs, VectorValues &sol) ;
/** output = \f$ R_t^{-T} \f$ input */
void transposeBackSubstitute(const VectorValues &rhs, VectorValues &sol) ;
/** return \f$ R_t^{-1} y + x_t \f$ */
VectorValues transform(const VectorValues &y);
/** naively split a gaussian factor graph into tree and constraint parts
* Note: This function has to be refined for your graph/application */
boost::tuple<GaussianFactorGraph::shared_ptr, FactorGraph<JacobianFactor>::shared_ptr>
splitGraph(const GaussianFactorGraph &gfg);
};
}

4
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -26,9 +26,9 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar) : const sharedBayesNet& Rc1, const sharedValues& xbar) :
Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
} }
/* ************************************************************************* */ /* ************************************************************************* */

9
gtsam/linear/SubgraphPreconditioner.h
View File

@ -40,7 +40,7 @@ namespace gtsam {
typedef boost::shared_ptr<const Errors> sharedErrors; typedef boost::shared_ptr<const Errors> sharedErrors;
private: private:
sharedFG Ab1_, Ab2_; sharedFG Ab2_;
sharedBayesNet Rc1_; sharedBayesNet Rc1_;
sharedValues xbar_; ///< A1 \ b1 sharedValues xbar_; ///< A1 \ b1
sharedErrors b2bar_; ///< A2*xbar - b2 sharedErrors b2bar_; ///< A2*xbar - b2
@ -51,16 +51,11 @@ namespace gtsam {
/** /**
* Constructor * Constructor
* @param Ab1: the Graph A1*x=b1
* @param Ab2: the Graph A2*x=b2 * @param Ab2: the Graph A2*x=b2
* @param Rc1: the Bayes Net R1*x=c1 * @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to R1*x=c1 * @param xbar: the solution to R1*x=c1
*/ */
SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
const sharedBayesNet& Rc1, const sharedValues& xbar);
/** Access Ab1 */
const sharedFG& Ab1() const { return Ab1_; }
/** Access Ab2 */ /** Access Ab2 */
const sharedFG& Ab2() const { return Ab2_; } const sharedFG& Ab2() const { return Ab2_; }

80
gtsam/linear/SubgraphSolver-inl.h
View File

@ -1,80 +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
//
// * -------------------------------------------------------------------------- */
//
//#pragma once
//
//#include <boost/foreach.hpp>
//
//#include <gtsam_unstable/linear/iterative-inl.h>
//#include <gtsam/inference/graph-inl.h>
//#include <gtsam/inference/EliminationTree.h>
//
//namespace gtsam {
//
//template<class GRAPH, class LINEAR, class KEY>
//void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
//
// GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
// GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
//
// if (parameters_->verbosity()) cout << "split the graph ...";
// split(pairs_, *graph, *Ab1, *Ab2) ;
// if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
//
// // // Add a HardConstraint to the root, otherwise the root will be singular
// // Key root = keys.back();
// // T_.addHardConstraint(root, theta0[root]);
// //
// // // compose the approximate solution
// // theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
//
// LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!!
// SubgraphPreconditioner::sharedBayesNet Rc1 =
// EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate(&EliminateQR);
// SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
//
// pc_ = boost::make_shared<SubgraphPreconditioner>(
// Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
//}
//
//template<class GRAPH, class LINEAR, class KEY>
//VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
//
// // preconditioned conjugate gradient
// VectorValues zeros = pc_->zero();
// VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>
// (*pc_, zeros, *parameters_);
//
// boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
// *xbar = pc_->x(ybar);
// return xbar;
//}
//
//template<class GRAPH, class LINEAR, class KEY>
//void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values& theta0) {
// // generate spanning tree
// PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
//
// // make the ordering
// list<KEY> keys = predecessorMap2Keys(tree_);
// ordering_ = boost::make_shared<Ordering>(list<Key>(keys.begin(), keys.end()));
//
// // build factor pairs
// pairs_.clear();
// typedef pair<KEY,KEY> EG ;
// BOOST_FOREACH( const EG &eg, tree_ ) {
// Key key1 = Key(eg.first),
// key2 = Key(eg.second) ;
// pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
// }
//}
//
//} // \namespace gtsam

174
gtsam/linear/SubgraphSolver.cpp
View File

@ -19,103 +19,171 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph-inl.h> #include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTree.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <list> #include <list>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters)
: parameters_(parameters) : parameters_(parameters)
{ {
JacobianFactorGraph::shared_ptr jfg = dynamicCastFactors(gfg);
initialize(*jfg);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &jfg, const Parameters &parameters)
: parameters_(parameters)
{
initialize(*jfg);
}
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(); /**************************************************************************************************/
GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>(); SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters)
: parameters_(parameters) {
boost::tie(Ab1, Ab2) = splitGraph(gfg) ; GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1)->eliminate(&EliminateQR);
JacobianFactorGraph::shared_ptr Ab2Jacobian = dynamicCastFactors(Ab2);
initialize(Rc1, Ab2Jacobian);
}
if (parameters_.verbosity()) /**************************************************************************************************/
cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1,
const JacobianFactorGraph::shared_ptr &Ab2, const Parameters &parameters)
// // Add a HardConstraint to the root, otherwise the root will be singular : parameters_(parameters) {
// Key root = keys.back();
// T_.addHardConstraint(root, theta0[root]);
//
// // compose the approximate solution
// theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR); GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); initialize(Rc1, Ab2);
// Convert or cast Ab1 to JacobianFactors
boost::shared_ptr<FactorGraph<JacobianFactor> > Ab1Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
Ab1Jacobians->reserve(Ab1->size());
BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab1) {
if(boost::shared_ptr<JacobianFactor> jf =
boost::dynamic_pointer_cast<JacobianFactor>(factor))
Ab1Jacobians->push_back(jf);
else
Ab1Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
} }
// Convert or cast Ab2 to JacobianFactors /**************************************************************************************************/
boost::shared_ptr<FactorGraph<JacobianFactor> > Ab2Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >(); SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
Ab1Jacobians->reserve(Ab2->size()); const Parameters &parameters) : parameters_(parameters)
BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab2) { {
if(boost::shared_ptr<JacobianFactor> jf = JacobianFactorGraph::shared_ptr Ab2Jacobians = dynamicCastFactors(Ab2);
boost::dynamic_pointer_cast<JacobianFactor>(factor)) initialize(Rc1, Ab2Jacobians);
Ab2Jacobians->push_back(jf);
else
Ab2Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
} }
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab1Jacobians, Ab2Jacobians, Rc1, xbar); /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const JacobianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters)
{
initialize(Rc1, Ab2);
} }
VectorValues SubgraphSolver::optimize() { VectorValues SubgraphSolver::optimize() {
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_); VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_);
return pc_->x(ybar); return pc_->x(ybar);
} }
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> void SubgraphSolver::initialize(const JacobianFactorGraph &jfg)
SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) { {
JacobianFactorGraph::shared_ptr Ab1 = boost::make_shared<JacobianFactorGraph>(),
Ab2 = boost::make_shared<JacobianFactorGraph>();
VariableIndex index(gfg); boost::tie(Ab1, Ab2) = splitGraph(jfg) ;
size_t n = index.size(); if (parameters_.verbosity())
std::vector<bool> connected(n, false); cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl;
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
JacobianFactorGraph::shared_ptr Ab2Jacobians = convertToJacobianFactorGraph(*Ab2);
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2Jacobians, Rc1, xbar);
}
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2)
{
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
boost::tuple<JacobianFactorGraph::shared_ptr, JacobianFactorGraph::shared_ptr>
SubgraphSolver::splitGraph(const JacobianFactorGraph &jfg) {
const VariableIndex index(jfg);
const size_t n = index.size(), m = jfg.size();
DisjointSet D(n) ;
JacobianFactorGraph::shared_ptr At(new JacobianFactorGraph());
JacobianFactorGraph::shared_ptr Ac( new JacobianFactorGraph());
size_t t = 0;
BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) {
if ( jf->keys().size() > 2 ) {
throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
}
bool augment = false ; bool augment = false ;
/* check whether this factor should be augmented to the "tree" graph */ /* check whether this factor should be augmented to the "tree" graph */
if ( gf->keys().size() == 1 ) augment = true; if ( jf->keys().size() == 1 ) augment = true;
else { else {
BOOST_FOREACH ( const Index key, *gf ) { const Index u = jf->keys()[0], v = jf->keys()[1],
if ( connected[key] == false ) { u_root = D.find(u), v_root = D.find(v);
augment = true ; if ( u_root != v_root ) {
connected[key] = true; t++; augment = true ;
D.makeUnion(u_root, v_root);
} }
} }
} if ( augment ) At->push_back(jf);
else Ac->push_back(jf);
if ( augment ) At->push_back(gf);
else Ac->push_back(gf);
} }
return boost::tie(At, Ac); return boost::tie(At, Ac);
} }
SubgraphSolver::DisjointSet::DisjointSet(const size_t n):n_(n),rank_(n,1),parent_(n) {
for ( Index i = 0 ; i < n ; ++i ) parent_[i] = i ;
}
Index SubgraphSolver::DisjointSet::makeUnion(const Index &u, const Index &v) {
Index u_root = find(u), v_root = find(v) ;
Index u_rank = rank(u), v_rank = rank(v) ;
if ( u_root != v_root ) {
if ( v_rank > u_rank ) {
parent_[u_root] = v_root ;
rank_[v_root] += rank_[u_root] ;
return v_root ;
}
else {
parent_[v_root] = u_root ;
rank_[u_root] += rank_[v_root] ;
return u_root ;
}
}
return u_root ;
}
Index SubgraphSolver::DisjointSet::find(const Index &u) {
vector<Index> path ;
Index x = u;
Index x_root = parent_[x] ;
// find the root, and keep the vertices along the path
while ( x != x_root ) {
path.push_back(x) ;
x = x_root ;
x_root = parent_[x] ;
}
// path compression
BOOST_FOREACH(const Index &i, path) {
rank_[i] = 1 ;
parent_[i] = x_root ;
}
return x_root ;
}
} // \namespace gtsam } // \namespace gtsam

69
gtsam/linear/SubgraphSolver.h
View File

@ -13,7 +13,9 @@
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
@ -25,31 +27,86 @@ public:
SubgraphSolverParameters() : Base() {} SubgraphSolverParameters() : Base() {}
}; };
/** /**
* A linear system solver using subgraph preconditioning conjugate gradient * This class implements the SPCG solver presented in Dellaert et al in IROS'10.
*
* Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into
* \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
* \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
* Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
*
* In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it
* with the least-squares variation of the conjugate gradient method.
*
* To use it in nonlinear optimization, please see the following example
*
* LevenbergMarquardtParams parameters;
* parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
* parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
* LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
* Values result = optimizer.optimize();
*
* \nosubgrouping
*/ */
class SubgraphSolver : public IterativeSolver { class SubgraphSolver : public IterativeSolver {
public: public:
typedef SubgraphSolverParameters Parameters; typedef SubgraphSolverParameters Parameters;
protected: protected:
Parameters parameters_; Parameters parameters_;
SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object
public: public:
/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters);
SubgraphSolver(const JacobianFactorGraph::shared_ptr &A, const Parameters &parameters);
/* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
/* The same as above, but the A1 is solved before */
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters);
virtual ~SubgraphSolver() {} virtual ~SubgraphSolver() {}
virtual VectorValues optimize () ; virtual VectorValues optimize () ;
protected: protected:
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> void initialize(const JacobianFactorGraph &jfg);
splitGraph(const GaussianFactorGraph &gfg) ; void initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2);
boost::tuple<JacobianFactorGraph::shared_ptr, JacobianFactorGraph::shared_ptr>
splitGraph(const JacobianFactorGraph &gfg) ;
public:
// a simplfied implementation of disjoint set data structure.
class DisjointSet {
protected:
size_t n_ ;
std::vector<size_t> rank_ ;
std::vector<Index> parent_ ;
public:
// initialize a disjoint set, point every vertex to itself
DisjointSet(const size_t n) ;
inline size_t size() const { return n_ ; }
// union the root of u and and the root of v, return the root of u and v
Index makeUnion(const Index &u, const Index &v) ;
// return the root of u
Index find(const Index &u) ;
// return the rank of x, which is defined as the cardinality of the set containing x
inline size_t rank(const Index &x) {return rank_[find(x)] ;}
};
}; };
} // namespace gtsam } // namespace gtsam

7
gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp
View File

@ -8,7 +8,6 @@
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h> #include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
#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>
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
@ -60,11 +59,7 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ
} }
else if ( params.isCG() ) { else if ( params.isCG() ) {
if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ...");
if ( boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params.iterativeParams)) { if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
SimpleSPCGSolver solver (gfg, *boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params.iterativeParams));
delta = solver.optimize();
}
else if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams)); SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams));
delta = solver.optimize(); delta = solver.optimize();
} }

4
tests/testSubgraphPreconditioner.cpp
View File

@ -120,7 +120,7 @@ TEST( SubgraphPreconditioner, system )
// Create Subgraph-preconditioned system // Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config // Create zero config
VectorValues zeros = VectorValues::Zero(xbar); VectorValues zeros = VectorValues::Zero(xbar);
@ -197,7 +197,7 @@ TEST( SubgraphPreconditioner, conjugateGradients )
// Create Subgraph-preconditioned system // Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config y0 and perturbed config y1 // Create zero config y0 and perturbed config y1
VectorValues y0 = VectorValues::Zero(xbar); VectorValues y0 = VectorValues::Zero(xbar);

106
tests/testSubgraphSolver.cpp Normal file
View File

@ -0,0 +1,106 @@
/* ----------------------------------------------------------------------------
* 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 testSubgraphSolver.cpp
* @brief Unit tests for SubgraphSolver
* @author Yong-Dian Jian
**/
#include <tests/smallExample.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
/** unnormalized error */
double error(const JacobianFactorGraph& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg)
total_error += factor->error(x);
return total_error;
}
/* ************************************************************************* */
TEST( SubgraphSolver, constructor1 )
{
// Build a planar graph
JacobianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
SubgraphSolverParameters parameters;
SubgraphSolver solver(Ab, parameters);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
/* ************************************************************************* */
TEST( SubgraphSolver, constructor2 )
{
// Build a planar graph
JacobianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
SubgraphSolverParameters parameters;
SubgraphSolver solver(Ab1_, Ab2_, parameters);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
/* ************************************************************************* */
TEST( SubgraphSolver, constructor3 )
{
// Build a planar graph
JacobianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR);
SubgraphSolverParameters parameters;
SubgraphSolver solver(Rc1, Ab2_, parameters);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */