From 7d132ef2177ceb85146d5bc7566b0b39bb2c254b Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sun, 3 Jun 2012 14:52:26 +0000 Subject: [PATCH] add a simple spcg implementation and revive the example --- examples/Pose2SLAMwSPCG_advanced.cpp | 109 --------- examples/Pose2SLAMwSPCG_easy.cpp | 101 ++++---- .../linear/IterativeOptimizationParameters.h | 6 +- gtsam/linear/IterativeSolver.h | 11 +- gtsam/linear/SimpleSPCGSolver.cpp | 230 ++++++++++++++++++ gtsam/linear/SimpleSPCGSolver.h | 87 +++++++ .../nonlinear/LevenbergMarquardtOptimizer.cpp | 4 +- 7 files changed, 366 insertions(+), 182 deletions(-) delete mode 100644 examples/Pose2SLAMwSPCG_advanced.cpp create mode 100644 gtsam/linear/SimpleSPCGSolver.cpp create mode 100644 gtsam/linear/SimpleSPCGSolver.h diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp deleted file mode 100644 index 12d17a6e9..000000000 --- a/examples/Pose2SLAMwSPCG_advanced.cpp +++ /dev/null @@ -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 - -#if ENABLE_SPCG - -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace pose2SLAM; - -typedef boost::shared_ptr sharedGraph ; -typedef boost::shared_ptr sharedValue ; -//typedef NonlinearOptimizer > SPCGOptimizer; - - -typedef SubgraphSolver Solver; -typedef boost::shared_ptr sharedSolver ; -typedef NonlinearOptimizer 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() ; - initial = boost::make_shared() ; - - // 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 diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp index e610d327c..5565b6206 100644 --- a/examples/Pose2SLAMwSPCG_easy.cpp +++ b/examples/Pose2SLAMwSPCG_easy.cpp @@ -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 - -#if ENABLE_SPCG +#include +#include +#include #include - -#include -#include - +#include 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(); + + 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 diff --git a/gtsam/linear/IterativeOptimizationParameters.h b/gtsam/linear/IterativeOptimizationParameters.h index b38f6ab1a..f6b1877ce 100644 --- a/gtsam/linear/IterativeOptimizationParameters.h +++ b/gtsam/linear/IterativeOptimizationParameters.h @@ -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_) {} diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 7b1c1e630..9b07d8e07 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -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 #include #include +#include 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_ ; } }; } diff --git a/gtsam/linear/SimpleSPCGSolver.cpp b/gtsam/linear/SimpleSPCGSolver.cpp new file mode 100644 index 000000000..4379d70e4 --- /dev/null +++ b/gtsam/linear/SimpleSPCGSolver.cpp @@ -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 +#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::shared_ptr ¶meters) + : Base(parameters) +{ + std::vector 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 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::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 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::shared_ptr SimpleSPCGSolver::transform(const VectorValues &y) { + VectorValues::shared_ptr x = boost::make_shared(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)); + } + +// gfg.print("gfg"); +// At->print("At"); +// Ac->print("Ac"); + + return boost::tie(At, Ac); +} + + + + + + +} diff --git a/gtsam/linear/SimpleSPCGSolver.h b/gtsam/linear/SimpleSPCGSolver.h new file mode 100644 index 000000000..416826cc7 --- /dev/null +++ b/gtsam/linear/SimpleSPCGSolver.h @@ -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 +#include +#include +#include + +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 shared_ptr; + +protected: + + size_t nVar_ ; /* number of variables */ + size_t nAc_ ; /* number of factors in Ac */ + + /* for SPCG */ + FactorGraph::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::shared_ptr> + splitGraph(const GaussianFactorGraph &gfg); + +}; + +} diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index f1f5ab1df..e1133d86e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -21,6 +21,7 @@ #include // For NegativeMatrixException #include #include +#include using namespace std; @@ -71,7 +72,8 @@ void LevenbergMarquardtOptimizer::iterate() { delta = gtsam::optimize(*EliminationTree::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");