diff --git a/examples/Makefile.am b/examples/Makefile.am index cbc24d00e..3f7de2b33 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -16,7 +16,6 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface -noinst_PROGRAMS += Pose2SLAMwCG # Solves a simple Pose2 SLAM example with easy CG solver noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index 35a2368df..35ee3efcf 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -22,7 +22,6 @@ int main(int argc, char** argv) { Key x1(1), x2(2), x3(3); /* 1. create graph container and add factors to it */ - //Graph graph; Graph graph ; /* 2.a add prior */ diff --git a/examples/Pose2SLAMwCG.cpp b/examples/Pose2SLAMwCG.cpp deleted file mode 100644 index 2d2aa0a30..000000000 --- a/examples/Pose2SLAMwCG.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Pose2SLAMwCG.cpp - * - * Created on: Oct 27, 2010 - * Author: Yong-Dian Jian - */ - -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/* - * Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface - */ - - -#include - -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace pose2SLAM; - -Graph graph; -Values initial; -Values result; - -/* ************************************************************************* */ -int main(void) { - - /* generate synthetic data */ - const SharedGaussian 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); - - // 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 */ - - - graph.print("full graph") ; - initial.print("initial estimate"); - - result = optimizePCG >(graph, initial); - //result = optimizeCG(graph, initial); - result.print("final result") ; - return 0 ; -} - diff --git a/gtsam/linear/ConjugateGradientSolver-inl.h b/gtsam/linear/ConjugateGradientSolver-inl.h deleted file mode 100644 index 9dd0b35c1..000000000 --- a/gtsam/linear/ConjugateGradientSolver-inl.h +++ /dev/null @@ -1,10 +0,0 @@ -/* - * ConjugateGradientSolver-inl.h - * - * Created on: Oct 24, 2010 - * Author: Yong-Dian Jian - */ - -#pragma once - -#include diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h deleted file mode 100644 index 2c8d70134..000000000 --- a/gtsam/linear/ConjugateGradientSolver.h +++ /dev/null @@ -1,135 +0,0 @@ -/* - * ConjugateGradientSolver.h - * - * Created on: Oct 21, 2010 - * Author: Yong-Dian Jian - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - - template - class ConjugateGradientSolver : public IterativeSolver { - - protected: - - typedef boost::shared_ptr sharedGRAPH ; - typedef boost::shared_ptr sharedLINEAR ; - typedef boost::shared_ptr sharedVALUES ; - typedef boost::shared_ptr sharedVectorValues ; - - const LINEAR *ptr_; - sharedVectorValues zeros_; - - public: - - typedef boost::shared_ptr shared_ptr ; - - // suggested constructor - ConjugateGradientSolver(const GRAPH &graph, const VALUES &initial, const Ordering &ordering, const Parameters ¶meters = Parameters()): - IterativeSolver(parameters), ptr_(0), zeros_(boost::make_shared(initial.zero(ordering))) {} - - // to be compatible to NonlinearOptimizer, but shouldn't be called... - ConjugateGradientSolver(const LINEAR &GFG) { - std::cout << "[ConjugateGradientSolver] Unexpected usage.." << std::endl; - throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); - } - - // copy constructor - ConjugateGradientSolver(const ConjugateGradientSolver& solver) : IterativeSolver(solver), ptr_(solver.ptr_), zeros_(solver.zeros_){} - - shared_ptr update(const LINEAR &graph) const { - return boost::make_shared(parameters_, &graph, zeros_) ; - } - - VectorValues::shared_ptr optimize() const { - VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, *parameters_); - return boost::make_shared(x) ; - } - - - // for NonlinearOptimizer update only - ConjugateGradientSolver(const sharedParameters parameters, - const LINEAR *ptr, - const sharedVectorValues zeros): - IterativeSolver(parameters), ptr_(ptr), zeros_(zeros) {} - - private: - // shouldn't be used - ConjugateGradientSolver(){} - }; - - template - class PreconditionedConjugateGradientSolver : public IterativeSolver { - - protected: - - typedef boost::shared_ptr sharedGRAPH ; - typedef boost::shared_ptr sharedLINEAR ; - typedef boost::shared_ptr sharedPreconditioner ; - typedef boost::shared_ptr sharedVALUES ; - typedef boost::shared_ptr sharedVectorValues ; - - const LINEAR *ptr_; - sharedPreconditioner pc_ ; - sharedVectorValues zeros_; - public: - - typedef boost::shared_ptr shared_ptr ; - - // suggested constructor - PreconditionedConjugateGradientSolver( - const GRAPH &graph, - const VALUES &initial, - const Ordering &ordering, - const Parameters ¶meters = Parameters()): - IterativeSolver(parameters), ptr_(0), pc_(), zeros_(boost::make_shared(initial.zero(ordering))) {} - - // to be compatible to NonlinearOptimizer, but shouldn't be called... - PreconditionedConjugateGradientSolver(const LINEAR &GFG) { - std::cout << "[PreconditionedConjugateGradientSolver] Unexpected usage.." << std::endl; - throw std::runtime_error("PreconditionedConjugateGradientSolver: gaussian factor graph initialization not supported"); - } - - // copy - PreconditionedConjugateGradientSolver(const PreconditionedConjugateGradientSolver& solver) : - IterativeSolver(solver), ptr_(solver.ptr_), pc_(solver.pc_), zeros_(solver.zeros_){} - - // called by NonlinearOptimizer - shared_ptr update(const LINEAR &graph) const { - - return boost::make_shared( - parameters_, - &graph, - boost::make_shared(&graph, parameters_, zeros_), - zeros_) ; - } - - // optimize with preconditioned conjugate gradient - VectorValues::shared_ptr optimize() const { - VectorValues x = preconditionedConjugateGradientDescent - (*ptr_, *pc_, *zeros_, *parameters_); - return boost::make_shared(x) ; - } - - PreconditionedConjugateGradientSolver( - const sharedParameters parameters, - const LINEAR *ptr, - const sharedPreconditioner pc, - const sharedVectorValues zeros): - IterativeSolver(parameters), ptr_(ptr), pc_(pc), zeros_(zeros) {} - private: - - // shouldn't be used - PreconditionedConjugateGradientSolver(){} - }; - - -} // nsamespace gtsam diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index a861adcf6..68347ce6e 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -36,7 +36,7 @@ check_PROGRAMS += tests/testGaussianJunctionTree # Iterative Methods headers += iterative-inl.h sources += iterative.cpp SubgraphPreconditioner.cpp -headers += IterativeOptimizationParameters.h IterativeSolver.h ConjugateGradientSolver.h ConjugateGradientSolver-inl.h +headers += IterativeSolver.h IterativeOptimizationParameters.h headers += SubgraphSolver.h SubgraphSolver-inl.h #sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp #check_PROGRAMS += tests/testBayesNetPreconditioner diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h deleted file mode 100644 index 9cf8c0de0..000000000 --- a/gtsam/linear/Preconditioner.h +++ /dev/null @@ -1,88 +0,0 @@ -/* - * Preconditioner.h - * - * Created on: Oct 27, 2010 - * Author: Yong-Dian Jian - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - -template -class Preconditioner { - -public: - typedef IterativeOptimizationParameters Parameters; - typedef boost::shared_ptr sharedParameters; - typedef boost::shared_ptr sharedLinear; - typedef boost::shared_ptr sharedValues ; - -public: - Preconditioner(const LINEAR *ptr, const sharedParameters parameters, const sharedValues zeros): - pLinear_(ptr), parameters_(parameters), zeros_(zeros){} - - void solve(const VALUES &values, VALUES &result) const { result = values ; } - void solveTranspose(const VALUES &values, VALUES &result) const { result = values ; } - -protected: - const LINEAR* pLinear_ ; - const sharedParameters parameters_ ; - const sharedValues zeros_; - -private: - Preconditioner(){} -}; - -template -class JacobiPreconditioner : public Preconditioner { - -public: - - typedef Preconditioner Base ; - typedef IterativeOptimizationParameters Parameters; - typedef boost::shared_ptr sharedParameters; - typedef boost::shared_ptr sharedLinear; - typedef boost::shared_ptr sharedValues ; - - - JacobiPreconditioner(const LINEAR *ptr, const sharedParameters parameters, const sharedValues zeros): - Base(ptr, parameters, zeros), scale_(new VALUES(*zeros)) { - initialize() ; - } - - void initialize() { - Base::pLinear_->getDiagonalOfHessian(*scale_); - sqrt(*scale_) ; - - // check if any zero in scale_ - if ( anyZero(*scale_) ) { - std::cout << "[JacobiPreconditioner] some diagonal element of hessian is zero" << std::endl ; - throw std::runtime_error("JacobiPreconditioner"); - } - } - - void solve(const VALUES &values, VALUES &result) const { - //std::cout << "solve" << std::endl; - ediv(values, *scale_, result) ; - } - - void solveTranspose(const VALUES &values, VALUES &result) const { - //std::cout << "solveTranspose" << std::endl; - ediv(values, *scale_, result) ; - } - -protected: - sharedValues scale_ ; - -private: - JacobiPreconditioner(){} -}; - -} diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 3f1da4669..157fc0806 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -19,11 +19,8 @@ #pragma once #include -#include #include -#include -#include #include using namespace std; @@ -35,7 +32,7 @@ namespace gtsam { template struct CGState { - typedef IterativeSolver::Parameters Parameters; + typedef IterativeOptimizationParameters Parameters; const Parameters ¶meters_; int k; @@ -127,7 +124,7 @@ namespace gtsam { V conjugateGradients( const S& Ab, V x, - const IterativeSolver::Parameters ¶meters, + const IterativeOptimizationParameters ¶meters, bool steepest = false) { CGState state(Ab, x, parameters, steepest); @@ -148,116 +145,6 @@ namespace gtsam { while (!state.step(Ab, x)) {} return x; } - - /* ************************************************************************* */ - // state for PCG method - template - class PCGState { - - public: - typedef IterativeSolver::Parameters Parameters; - typedef V Values; - typedef boost::shared_ptr sharedValues; - - protected: - const LINEAR &Ab_; - const PC &pc_ ; - V x_ ; - const Parameters ¶meters_; - bool steepest_; - - public: - /* ************************************************************************* */ - // Constructor - PCGState(const LINEAR& Ab, const PC &pc, const V &x0, const Parameters ¶meters, bool steep): - Ab_(Ab), pc_(pc), x_(x0), parameters_(parameters),steepest_(steep) {} - - V run() { - - // refer to Bjorck pp. 294 - V r = Ab_.allocateVectorValuesb() ; - V q = V::SameStructure(r) ; - - V p = V::SameStructure(x_) ; - V s = V::SameStructure(x_) ; - V t = V::SameStructure(x_) ; - V tmp = V::SameStructure(x_) ; - - // initial PCG - Ab_.residual(x_, r) ; - Ab_.transposeMultiply(r, tmp) ; - pc_.solveTranspose(tmp, s) ; - p = s ; - - double gamma = dot(s,s), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ; - - const double threshold = - ::max(parameters_.epsilon_abs(), - parameters_.epsilon() * parameters_.epsilon() * gamma); - - const int iMaxIterations = parameters_.maxIterations(); - const int iReset = parameters_.reset() ; - - if (parameters_.verbosity()) - cout << "PCG: epsilon = " << parameters_.epsilon() - << ", maxIterations = " << parameters_.maxIterations() - << ", ||g0||^2 = " << gamma - << ", threshold = " << threshold << endl; - - for ( int k = 0 ; k < iMaxIterations ; ++k ) { - - if ( gamma < threshold ) break ; - - if ( k % iReset == 0) { - Ab_.residual(x_, r) ; - Ab_.transposeMultiply(r, tmp) ; - pc_.solveTranspose(tmp, s) ; - p = s ; - gamma = dot(s,s) ; - } - - - pc_.solve(p, t) ; - Ab_.multiply(t, q) ; - alpha = gamma / dot(q,q) ; - axpy( alpha, t, x_) ; - axpy(-alpha, q, r) ; - Ab_.transposeMultiply(r, tmp) ; - pc_.solveTranspose(tmp, s) ; - new_gamma = dot(s,s) ; - - if (parameters_.verbosity()) - cout << "iteration " << k - << ": alpha = " << alpha - << ", dotg = " << new_gamma << endl; - - beta = new_gamma / gamma ; - scal(beta,p) ; - axpy(1.0,s,p) ; - gamma = new_gamma ; - } - - return x_ ; - } // function - - private: - PCGState(){} - - }; // PCGState Class - - /* ************************************************************************* */ - template - V preconditionedConjugateGradientDescent( - const LINEAR& Ab, - const PC& pc, - V x, - const IterativeSolver::Parameters ¶meters, - bool steepest = false) { - - PCGState state(Ab, pc, x, parameters, steepest); - return state.run() ; - } - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index 3894f8b0d..29d5cab12 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -20,8 +20,6 @@ #include #include #include -#include -#include #include #include @@ -38,31 +36,31 @@ namespace gtsam { /* ************************************************************************* */ - Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::Parameters & parameters) { + Vector steepestDescent(const System& Ab, const Vector& x, const IterativeOptimizationParameters & parameters) { return conjugateGradients (Ab, x, parameters, true); } - Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::Parameters & parameters) { + Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeOptimizationParameters & parameters) { return conjugateGradients (Ab, x, parameters); } /* ************************************************************************* */ - Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::Parameters & parameters) { + Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeOptimizationParameters & parameters) { System Ab(A, b); return conjugateGradients (Ab, x, parameters, true); } - Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::Parameters & parameters) { + Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeOptimizationParameters & parameters) { System Ab(A, b); return conjugateGradients (Ab, x, parameters); } /* ************************************************************************* */ - VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::Parameters & parameters) { + VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) { return conjugateGradients (fg, x, parameters, true); } - VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::Parameters & parameters) { + VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) { return conjugateGradients (fg, x, parameters); } diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 1bd042fc1..224c0ae44 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -20,8 +20,7 @@ #include #include -#include -#include +#include namespace gtsam { @@ -92,7 +91,7 @@ namespace gtsam { Vector steepestDescent( const System& Ab, const Vector& x, - const IterativeSolver::Parameters & parameters); + const IterativeOptimizationParameters & parameters); /** * Method of conjugate gradients (CG), System version @@ -100,7 +99,7 @@ namespace gtsam { Vector conjugateGradientDescent( const System& Ab, const Vector& x, - const IterativeSolver::Parameters & parameters); + const IterativeOptimizationParameters & parameters); /** convenience calls using matrices, will create System class internally: */ @@ -111,7 +110,7 @@ namespace gtsam { const Matrix& A, const Vector& b, const Vector& x, - const IterativeSolver::Parameters & parameters); + const IterativeOptimizationParameters & parameters); /** * Method of conjugate gradients (CG), Matrix version @@ -120,7 +119,7 @@ namespace gtsam { const Matrix& A, const Vector& b, const Vector& x, - const IterativeSolver::Parameters & parameters); + const IterativeOptimizationParameters & parameters); class GaussianFactorGraph; @@ -130,7 +129,7 @@ namespace gtsam { VectorValues steepestDescent( const GaussianFactorGraph& fg, const VectorValues& x, - const IterativeSolver::Parameters & parameters); + const IterativeOptimizationParameters & parameters); /** * Method of conjugate gradients (CG), Gaussian Factor Graph version @@ -138,7 +137,7 @@ namespace gtsam { VectorValues conjugateGradientDescent( const GaussianFactorGraph& fg, const VectorValues& x, - const IterativeSolver::Parameters & parameters); + const IterativeOptimizationParameters & parameters); } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h index 7d0bddf8c..51f7c3d7d 100644 --- a/gtsam/nonlinear/NonlinearOptimization-inl.h +++ b/gtsam/nonlinear/NonlinearOptimization-inl.h @@ -23,7 +23,6 @@ #include #include -#include #include #include #include @@ -70,52 +69,6 @@ namespace gtsam { return *result.values(); } - /** - * The cg solver - */ - template - T optimizeCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { - - typedef ConjugateGradientSolver Solver; - typedef boost::shared_ptr sharedSolver; - typedef NonlinearOptimizer Optimizer; - - Ordering::shared_ptr ordering = initialEstimate.orderingArbitrary() ; - sharedSolver solver = boost::make_shared(graph, initialEstimate, *ordering, IterativeOptimizationParameters()); - Optimizer optimizer( - boost::make_shared(graph), - boost::make_shared(initialEstimate), - ordering, - solver); - - // Levenberg-Marquardt - Optimizer result = optimizer.levenbergMarquardt(parameters); - return *result.values(); - } - - template - T optimizePCG(const G& graph, - const T& initialEstimate, - const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { - - typedef PreconditionedConjugateGradientSolver Solver; - typedef boost::shared_ptr sharedSolver; - typedef NonlinearOptimizer Optimizer; - - Ordering::shared_ptr ordering = initialEstimate.orderingArbitrary() ; - sharedSolver solver = boost::make_shared(graph, initialEstimate, *ordering, IterativeOptimizationParameters()); - Optimizer optimizer( - boost::make_shared(graph), - boost::make_shared(initialEstimate), - ordering, - solver); - - // Levenberg-Marquardt - Optimizer result = optimizer.levenbergMarquardt(parameters); - return *result.values(); - } - - /** * The sparse preconditioned conjucate gradient solver */ @@ -149,8 +102,6 @@ namespace gtsam { return optimizeSequential(graph, initialEstimate, parameters); case MULTIFRONTAL: return optimizeMultiFrontal(graph, initialEstimate, parameters); - case CG: - return optimizeCG(graph, initialEstimate, parameters); case SPCG: throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint"); } diff --git a/gtsam/nonlinear/NonlinearOptimization.h b/gtsam/nonlinear/NonlinearOptimization.h index e4107dd24..0209819a6 100644 --- a/gtsam/nonlinear/NonlinearOptimization.h +++ b/gtsam/nonlinear/NonlinearOptimization.h @@ -37,11 +37,9 @@ namespace gtsam { enum LinearSolver{ SEQUENTIAL, // Sequential elimination MULTIFRONTAL, // Multi-frontal elimination - CG, // vanilla conjugate gradient SPCG, // Subgraph Preconditioned Conjugate Gradient }; - /** * optimization that returns the values */