diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 77146d1e4..6030f7be6 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -52,7 +52,7 @@ namespace gtsam { public: IterativeSolver(){} virtual ~IterativeSolver() {} - virtual VectorValues::shared_ptr optimize () = 0; + virtual VectorValues optimize () = 0; }; } diff --git a/gtsam/linear/SimpleSPCGSolver.cpp b/gtsam/linear/SimpleSPCGSolver.cpp index addbc74dd..fa0fee82f 100644 --- a/gtsam/linear/SimpleSPCGSolver.cpp +++ b/gtsam/linear/SimpleSPCGSolver.cpp @@ -82,7 +82,7 @@ SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Paramet } /* implements the CGLS method in Section 7.4 of Bjork's book */ -VectorValues::shared_ptr SimpleSPCGSolver::optimize (const VectorValues &initial) { +VectorValues SimpleSPCGSolver::optimize (const VectorValues &initial) { VectorValues::shared_ptr x(new VectorValues(initial)); VectorValues r = VectorValues::Zero(*by_), @@ -180,10 +180,10 @@ void SimpleSPCGSolver::transposeBackSubstitute(const VectorValues &input, Vector } } -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); +VectorValues SimpleSPCGSolver::transform(const VectorValues &y) { + VectorValues x = VectorValues::Zero(y); + this->backSubstitute(y, x); + axpy(1.0, *xt_, x); return x; } diff --git a/gtsam/linear/SimpleSPCGSolver.h b/gtsam/linear/SimpleSPCGSolver.h index 097022c22..994815fa4 100644 --- a/gtsam/linear/SimpleSPCGSolver.h +++ b/gtsam/linear/SimpleSPCGSolver.h @@ -65,11 +65,11 @@ public: SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); virtual ~SimpleSPCGSolver() {} - virtual VectorValues::shared_ptr optimize () {return optimize(*y0_);} + virtual VectorValues optimize () {return optimize(*y0_);} protected: - VectorValues::shared_ptr optimize (const VectorValues &initial); + 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); @@ -87,7 +87,7 @@ protected: void transposeBackSubstitute(const VectorValues &rhs, VectorValues &sol) ; /** return \f$ R_t^{-1} y + x_t \f$ */ - VectorValues::shared_ptr transform(const VectorValues &y); + 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 */ diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 891585ee5..d2af81a6c 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -58,13 +58,10 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters Rc1, xbar); } -VectorValues::shared_ptr SubgraphSolver::optimize() { +VectorValues SubgraphSolver::optimize() { VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); - - boost::shared_ptr xbar = boost::make_shared() ; - *xbar = pc_->x(ybar); - return xbar; + return pc_->x(ybar); } boost::tuple diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index debb373f2..60104bbe0 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -44,7 +44,7 @@ public: SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); virtual ~SubgraphSolver() {} - virtual VectorValues::shared_ptr optimize () ; + virtual VectorValues optimize () ; protected: diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 3c9cffc7b..8a052a66f 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -79,6 +79,7 @@ namespace gtsam { /* ************************************************************************* */ // take a step, return true if converged bool step(const S& Ab, V& x) { + if ((++k) >= ((int)parameters_.maxIterations())) return true; //----------------------------------> @@ -91,9 +92,12 @@ namespace gtsam { // check for convergence double new_gamma = dot(g, g); + if (parameters_.verbosity() != ConjugateGradientParameters::SILENT) std::cout << "iteration " << k << ": alpha = " << alpha - << ", dotg = " << new_gamma << std::endl; + << ", dotg = " << new_gamma + << std::endl; + if (new_gamma < threshold) return true; // calculate new search direction @@ -124,9 +128,10 @@ namespace gtsam { if (parameters.verbosity() != ConjugateGradientParameters::SILENT) std::cout << "CG: epsilon = " << parameters.epsilon() - << ", maxIterations = " << parameters.maxIterations() - << ", ||g0||^2 = " << state.gamma - << ", threshold = " << state.threshold << std::endl; + << ", maxIterations = " << parameters.maxIterations() + << ", ||g0||^2 = " << state.gamma + << ", threshold = " << state.threshold + << std::endl; if ( state.gamma < state.threshold ) { if (parameters.verbosity() != ConjugateGradientParameters::SILENT) diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 4237f2632..5ffb366c2 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -36,8 +36,7 @@ namespace gtsam { * @param steepest flag, if true does steepest descent, not CG * */ template - V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, - size_t maxIterations, bool steepest = false); + V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, size_t maxIterations, bool steepest = false); /** * Helper class encapsulating the combined system |Ax-b_|^2 diff --git a/gtsam/nonlinear/GradientDescentOptimizer.h b/gtsam/nonlinear/GradientDescentOptimizer.h index 5caef0957..2407ee39f 100644 --- a/gtsam/nonlinear/GradientDescentOptimizer.h +++ b/gtsam/nonlinear/GradientDescentOptimizer.h @@ -108,7 +108,6 @@ public: cg_(cg) {} virtual ~ConjugateGradientOptimizer() {} - virtual Values optimize () ; }; @@ -223,8 +222,7 @@ V conjugateGradient(const S &system, const V &initial, const NonlinearOptimizerP // Maybe show output if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl; } while( ++iteration < params.maxIterations && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, - params.errorTol, prevError, currentError, params.verbosity)); + !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity)); // Printing if verbose if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index ddebdb498..5847986be 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -80,11 +80,11 @@ void LevenbergMarquardtOptimizer::iterate() { if ( boost::dynamic_pointer_cast(params_.iterativeParams)) { SimpleSPCGSolver solver (dampedSystem, *boost::dynamic_pointer_cast(params_.iterativeParams)); - delta = *solver.optimize(); + delta = solver.optimize(); } else if ( boost::dynamic_pointer_cast(params_.iterativeParams) ) { SubgraphSolver solver (dampedSystem, *boost::dynamic_pointer_cast(params_.iterativeParams)); - delta = *solver.optimize(); + delta = solver.optimize(); } else { throw runtime_error("LMSolver: special cg parameter type is not handled in LM solver ...");