diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 240361db7..36f2552a1 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -54,13 +54,9 @@ // for each variable, held in a Values container. #include -// ??? -#include #include #include - - using namespace std; using namespace gtsam; @@ -110,14 +106,6 @@ int main(int argc, char** argv) { parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; - { - parameters.iterativeParams = boost::make_shared(); - 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(); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); diff --git a/gtsam/linear/JacobianFactorGraph.cpp b/gtsam/linear/JacobianFactorGraph.cpp index 0ca3b8667..70c57e8df 100644 --- a/gtsam/linear/JacobianFactorGraph.cpp +++ b/gtsam/linear/JacobianFactorGraph.cpp @@ -115,7 +115,20 @@ JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gf 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(factor) ) { + jfg->push_back(jf); + } + else { + jfg->push_back(boost::make_shared(*factor)); + } + } + return jfg; +} } // namespace diff --git a/gtsam/linear/JacobianFactorGraph.h b/gtsam/linear/JacobianFactorGraph.h index cb27b507a..1164652c3 100644 --- a/gtsam/linear/JacobianFactorGraph.h +++ b/gtsam/linear/JacobianFactorGraph.h @@ -67,7 +67,9 @@ namespace gtsam { void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); 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); + /** convert the gaussian factors down to jacobian factors, may duplicate factors if it contains Hessian Factor */ + JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg); } diff --git a/gtsam/linear/SimpleSPCGSolver.cpp b/gtsam/linear/SimpleSPCGSolver.cpp deleted file mode 100644 index fa0fee82f..000000000 --- a/gtsam/linear/SimpleSPCGSolver.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -/* utility function */ -std::vector extractRowSpec_(const FactorGraph& jfg) { - std::vector spec; spec.reserve(jfg.size()); - BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) { - spec.push_back(jf->rows()); - } - return spec; -} - -std::vector extractColSpec_(const FactorGraph& gfg, const VariableIndex &index) { - const size_t n = index.size(); - std::vector 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 ¶meters) - : Base(), parameters_(parameters) -{ - std::vector 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 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::Create(*At)->eliminate(EliminateQR); - xt_ = boost::make_shared(gtsam::optimize(*Rt_)); - - /* initial value for the lspcg method */ - y0_ = boost::make_shared(VectorValues::Zero(colSpec)); - - /* the right hand side of the new system */ - by_ = boost::make_shared(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::Zero(colSpec)); - tmpB_ = boost::make_shared(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 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().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 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::shared_ptr> -SimpleSPCGSolver::splitGraph(const GaussianFactorGraph &gfg) { - - VariableIndex index(gfg); - size_t n = index.size(); - std::vector connected(n, false); - - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - FactorGraph::shared_ptr Ac( new FactorGraph()); - - 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(gf)); - } - - return boost::tie(At, Ac); -} - - - - - - -} diff --git a/gtsam/linear/SimpleSPCGSolver.h b/gtsam/linear/SimpleSPCGSolver.h deleted file mode 100644 index 994815fa4..000000000 --- a/gtsam/linear/SimpleSPCGSolver.h +++ /dev/null @@ -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 -#include -#include -#include -#include - -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 shared_ptr; - -protected: - - size_t nVar_ ; ///< number of variables \f$ x \f$ - size_t nAc_ ; ///< number of factors in \f$ A_c \f$ - FactorGraph::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 ¶meters); - 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::shared_ptr> - splitGraph(const GaussianFactorGraph &gfg); -}; - -} diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 91a98a263..b33adbc38 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -26,9 +26,9 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, + SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, 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))) { } /* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 8c1fcc837..b109368af 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -40,7 +40,7 @@ namespace gtsam { typedef boost::shared_ptr sharedErrors; private: - sharedFG Ab1_, Ab2_; + sharedFG Ab2_; sharedBayesNet Rc1_; sharedValues xbar_; ///< A1 \ b1 sharedErrors b2bar_; ///< A2*xbar - b2 @@ -51,16 +51,11 @@ namespace gtsam { /** * Constructor - * @param Ab1: the Graph A1*x=b1 * @param Ab2: the Graph A2*x=b2 * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, - const sharedBayesNet& Rc1, const sharedValues& xbar); - - /** Access Ab1 */ - const sharedFG& Ab1() const { return Ab1_; } + SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); /** Access Ab2 */ const sharedFG& Ab2() const { return Ab2_; } diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h deleted file mode 100644 index 28b2caaac..000000000 --- a/gtsam/linear/SubgraphSolver-inl.h +++ /dev/null @@ -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 -// -//#include -//#include -//#include -// -//namespace gtsam { -// -//template -//void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { -// -// GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); -// GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); -// -// 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 (T_, tree, theta0[root]); -// -// LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!! -// SubgraphPreconditioner::sharedBayesNet Rc1 = -// EliminationTree::Create(sacrificialAb1)->eliminate(&EliminateQR); -// SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); -// -// pc_ = boost::make_shared( -// Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); -//} -// -//template -//VectorValues::shared_ptr SubgraphSolver::optimize() const { -// -// // preconditioned conjugate gradient -// VectorValues zeros = pc_->zero(); -// VectorValues ybar = conjugateGradients -// (*pc_, zeros, *parameters_); -// -// boost::shared_ptr xbar = boost::make_shared() ; -// *xbar = pc_->x(ybar); -// return xbar; -//} -// -//template -//void SubgraphSolver::initialize(const GRAPH& G, const Values& theta0) { -// // generate spanning tree -// PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); -// -// // make the ordering -// list keys = predecessorMap2Keys(tree_); -// ordering_ = boost::make_shared(list(keys.begin(), keys.end())); -// -// // build factor pairs -// pairs_.clear(); -// typedef pair EG ; -// BOOST_FOREACH( const EG &eg, tree_ ) { -// Key key1 = Key(eg.first), -// key2 = Key(eg.second) ; -// pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; -// } -//} -// -//} // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 9b37d9a4b..14a0cc174 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -19,103 +19,171 @@ #include #include #include - #include #include - #include using namespace std; namespace gtsam { +/**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) : parameters_(parameters) { + JacobianFactorGraph::shared_ptr jfg = dynamicCastFactors(gfg); + initialize(*jfg); +} +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &jfg, const Parameters ¶meters) + : parameters_(parameters) +{ + initialize(*jfg); +} - GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); - GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters) + : parameters_(parameters) { - boost::tie(Ab1, Ab2) = splitGraph(gfg) ; + GaussianBayesNet::shared_ptr Rc1 = EliminationTree::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; - - // // 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 (T_, tree, theta0[root]); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, + const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) + : parameters_(parameters) { GaussianBayesNet::shared_ptr Rc1 = EliminationTree::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 > Ab1Jacobians = boost::make_shared >(); - Ab1Jacobians->reserve(Ab1->size()); - BOOST_FOREACH(const boost::shared_ptr& factor, *Ab1) { - if(boost::shared_ptr jf = - boost::dynamic_pointer_cast(factor)) - Ab1Jacobians->push_back(jf); - else - Ab1Jacobians->push_back(boost::make_shared(*factor)); - } - - // Convert or cast Ab2 to JacobianFactors - boost::shared_ptr > Ab2Jacobians = boost::make_shared >(); - Ab1Jacobians->reserve(Ab2->size()); - BOOST_FOREACH(const boost::shared_ptr& factor, *Ab2) { - if(boost::shared_ptr jf = - boost::dynamic_pointer_cast(factor)) - Ab2Jacobians->push_back(jf); - else - Ab2Jacobians->push_back(boost::make_shared(*factor)); - } +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, + const Parameters ¶meters) : parameters_(parameters) +{ + JacobianFactorGraph::shared_ptr Ab2Jacobians = dynamicCastFactors(Ab2); + initialize(Rc1, Ab2Jacobians); +} - pc_ = boost::make_shared(Ab1Jacobians, Ab2Jacobians, Rc1, xbar); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) +{ + initialize(Rc1, Ab2); } VectorValues SubgraphSolver::optimize() { - VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } -boost::tuple -SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) { +void SubgraphSolver::initialize(const JacobianFactorGraph &jfg) +{ + JacobianFactorGraph::shared_ptr Ab1 = boost::make_shared(), + Ab2 = boost::make_shared(); - VariableIndex index(gfg); - size_t n = index.size(); - std::vector connected(n, false); + boost::tie(Ab1, Ab2) = splitGraph(jfg) ; + if (parameters_.verbosity()) + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); + GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); + VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + JacobianFactorGraph::shared_ptr Ab2Jacobians = convertToJacobianFactorGraph(*Ab2); + pc_ = boost::make_shared(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(Ab2, Rc1, xbar); +} + +boost::tuple +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 ; /* 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 { - BOOST_FOREACH ( const Index key, *gf ) { - if ( connected[key] == false ) { - augment = true ; - connected[key] = true; - } + const Index u = jf->keys()[0], v = jf->keys()[1], + u_root = D.find(u), v_root = D.find(v); + if ( u_root != v_root ) { + t++; augment = true ; + D.makeUnion(u_root, v_root); } } - - if ( augment ) At->push_back(gf); - else Ac->push_back(gf); + if ( augment ) At->push_back(jf); + else Ac->push_back(jf); } 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 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 diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 60104bbe0..04b1cb127 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -13,7 +13,9 @@ #include #include +#include #include +#include #include @@ -25,31 +27,86 @@ public: 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(); + * LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + * Values result = optimizer.optimize(); + * + * \nosubgrouping */ class SubgraphSolver : public IterativeSolver { public: - typedef SubgraphSolverParameters Parameters; protected: - Parameters parameters_; SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object public: + /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters); + SubgraphSolver(const JacobianFactorGraph::shared_ptr &A, const Parameters ¶meters); + + /* 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 ¶meters); + SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + + /* The same as above, but the A1 is solved before */ + SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); - SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); virtual ~SubgraphSolver() {} virtual VectorValues optimize () ; protected: - boost::tuple - splitGraph(const GaussianFactorGraph &gfg) ; + void initialize(const JacobianFactorGraph &jfg); + void initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2); + + boost::tuple + splitGraph(const JacobianFactorGraph &gfg) ; + +public: + + // a simplfied implementation of disjoint set data structure. + class DisjointSet { + protected: + size_t n_ ; + std::vector rank_ ; + std::vector 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 diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index af3f70ddc..9d85f26fe 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -60,11 +59,7 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ } else if ( params.isCG() ) { if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); - if ( boost::dynamic_pointer_cast(params.iterativeParams)) { - SimpleSPCGSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); - delta = solver.optimize(); - } - else if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { + if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); delta = solver.optimize(); } diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 60f737d67..b0a008478 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -120,7 +120,7 @@ TEST( SubgraphPreconditioner, system ) // Create Subgraph-preconditioned system VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); + SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Create zero config VectorValues zeros = VectorValues::Zero(xbar); @@ -197,7 +197,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) // Create Subgraph-preconditioned system 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 VectorValues y0 = VectorValues::Zero(xbar); diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp new file mode 100644 index 000000000..04e0554e5 --- /dev/null +++ b/tests/testSubgraphSolver.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +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::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); } +/* ************************************************************************* */