diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index c5c2efc66..46a3787f2 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -68,7 +68,6 @@ int main(void) { 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; diff --git a/gtsam/linear/IterativeOptimizationParameters.h b/gtsam/linear/IterativeOptimizationParameters.h index f6b1877ce..88303fa40 100644 --- a/gtsam/linear/IterativeOptimizationParameters.h +++ b/gtsam/linear/IterativeOptimizationParameters.h @@ -113,7 +113,9 @@ struct PreconditionerParameters { std::cout << "PreconditionerParameters: " << "kernel = " << kernelStr[kernel_] - << ", type = " << typeStr[type_] << std::endl; + << ", type = " << typeStr[type_] + << ", verbosity = " << verbosity_ + << std::endl; combinatorial_.print(); } }; @@ -167,6 +169,7 @@ struct ConjugateGradientParameters { << ", eps_rel = " << epsilon_rel_ << ", eps_abs = " << epsilon_abs_ << ", degree = " << degree_ + << ", verbosity = " << verbosity_ << std::endl; } }; @@ -180,8 +183,8 @@ public: PreconditionerParameters preconditioner_; ConjugateGradientParameters cg_; - enum Kernel { PCG = 0, LSPCG } kernel_ ; /* Iterative Method Kernel */ - enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */ + enum Kernel { PCG = 0, LSPCG = 1 } kernel_ ; ///< Iterative Method Kernel + enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; ///< Verbosity public: @@ -221,7 +224,8 @@ public: const std::string kernelStr[2] = {"pcg", "lspcg"}; std::cout << s << std::endl << "IterativeOptimizationParameters: " - << "kernel = " << kernelStr[kernel_] << std::endl; + << "kernel = " << kernelStr[kernel_] + << ", verbosity = " << verbosity_ << std::endl; cg_.print(); preconditioner_.print(); diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 9b07d8e07..58e83d595 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -11,9 +11,8 @@ #pragma once -#include #include -#include +#include namespace gtsam { @@ -21,25 +20,23 @@ class IterativeSolver { public: - typedef boost::shared_ptr shared_ptr; typedef IterativeOptimizationParameters Parameters; protected: - Parameters::shared_ptr parameters_ ; + Parameters parameters_ ; public: - IterativeSolver(): parameters_(new Parameters()) {} + IterativeSolver(): parameters_() {} IterativeSolver(const IterativeSolver &solver) : parameters_(solver.parameters_) {} - IterativeSolver(const Parameters::shared_ptr& parameters) : parameters_(parameters) {} - IterativeSolver(const Parameters ¶meters) : parameters_(new Parameters(parameters)) {} + IterativeSolver(const Parameters ¶meters) : parameters_(parameters) {} virtual ~IterativeSolver() {} virtual VectorValues::shared_ptr optimize () = 0; - inline Parameters::shared_ptr parameters() { return parameters_ ; } + inline const Parameters& parameters() const { return parameters_ ; } }; } diff --git a/gtsam/linear/SimpleSPCGSolver.cpp b/gtsam/linear/SimpleSPCGSolver.cpp index e7cef9e64..ae7c9581c 100644 --- a/gtsam/linear/SimpleSPCGSolver.cpp +++ b/gtsam/linear/SimpleSPCGSolver.cpp @@ -42,7 +42,7 @@ std::vector extractColSpec_(const FactorGraph& gfg, cons return spec; } -SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr ¶meters) +SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) : Base(parameters) { std::vector colSpec = extractColSpec_(gfg, VariableIndex(gfg)); @@ -97,13 +97,14 @@ VectorValues::shared_ptr SimpleSPCGSolver::optimize (const VectorValues &initial 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(); + ::max(parameters_.epsilon_abs(), + parameters_.epsilon() * parameters_.epsilon() * gamma); + const size_t iMaxIterations = parameters_.maxIterations(); + const ConjugateGradientParameters::Verbosity verbosity = parameters_.cg_.verbosity(); - if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR ) - cout << "[SimpleSPCGSolver] epsilon = " << parameters_->epsilon() - << ", max = " << parameters_->maxIterations() + if ( verbosity >= ConjugateGradientParameters::ERROR ) + cout << "[SimpleSPCGSolver] epsilon = " << parameters_.epsilon() + << ", max = " << parameters_.maxIterations() << ", ||r0|| = " << std::sqrt(gamma) << ", threshold = " << threshold << std::endl; @@ -120,14 +121,14 @@ VectorValues::shared_ptr SimpleSPCGSolver::optimize (const VectorValues &initial p.vector() = s.vector() + beta * p.vector(); gamma = new_gamma ; - if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR) { + if ( verbosity >= ConjugateGradientParameters::ERROR) { cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl; } if ( gamma < threshold ) break ; } // k - if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR ) + if ( verbosity >= ConjugateGradientParameters::ERROR ) cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl; /* transform y back to x */ diff --git a/gtsam/linear/SimpleSPCGSolver.h b/gtsam/linear/SimpleSPCGSolver.h index 04729fc1a..570169e7b 100644 --- a/gtsam/linear/SimpleSPCGSolver.h +++ b/gtsam/linear/SimpleSPCGSolver.h @@ -55,7 +55,7 @@ protected: public: - SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr ¶meters); + SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); virtual ~SimpleSPCGSolver() {} virtual VectorValues::shared_ptr optimize () {return optimize(*y0_);} diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index e1133d86e..17e92c466 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -72,7 +72,8 @@ void LevenbergMarquardtOptimizer::iterate() { delta = gtsam::optimize(*EliminationTree::Create(dampedSystem)->eliminate(params_.getEliminationFunction())); } else if ( params_.isCG() ) { - SimpleSPCGSolver solver(dampedSystem, *params_.iterativeParams); + IterativeOptimizationParameters::shared_ptr params(!params_.iterativeParams ? boost::make_shared() : params_.iterativeParams); + SimpleSPCGSolver solver(dampedSystem, *params); delta = *solver.optimize(); } else { diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index ea54dd344..156440f06 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -31,13 +31,13 @@ public: MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - CHOLMOD, /* Experimental Flag */ CG, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ }; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) - boost::optional iterativeParams; ///< The container for iterativeOptimization parameters. + IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {}