diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 5f2f2d7d4..b808a973c 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -102,6 +102,18 @@ void NonlinearOptimizer::defaultOptimize() { cout << "Terminating because reached maximum iterations" << endl; } +/* ************************************************************************* */ +const Values& NonlinearOptimizer::optimizeSafely() { + static const Values empty; + try { + defaultOptimize(); + return values(); + } catch (...) { + // uncaught exception, returning empty result + return empty; + } +} + /* ************************************************************************* */ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, @@ -143,6 +155,7 @@ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold } return converged; } +/* ************************************************************************* */ } diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 8a7670f85..44b352938 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -175,6 +175,22 @@ Values::const_shared_ptr result = DoglegOptimizer(graph, initialValues, params). */ class NonlinearOptimizer { +protected: + + NonlinearFactorGraph graph_; + + /** A default implementation of the optimization loop, which calls iterate() + * until checkConvergence returns true. + */ + void defaultOptimize(); + + virtual const NonlinearOptimizerState& _state() const = 0; + + virtual const NonlinearOptimizerParams& _params() const = 0; + + /** Constructor for initial construction of base classes. */ + NonlinearOptimizer(const NonlinearFactorGraph& graph) : graph_(graph) {} + public: /** A shared pointer to this class */ @@ -194,10 +210,21 @@ public: */ virtual const Values& optimize() { defaultOptimize(); return values(); } + /** + * Optimize, but return empty result if any uncaught exception is thrown + * Intended for MATLAB. In C++, use above and catch exceptions. + * No message is printed: it is up to the caller to check the result + * @param optimizer a non-linear optimizer + */ + const Values& optimizeSafely(); + + /// return error double error() const { return _state().error; } + /// return number of iterations unsigned int iterations() const { return _state().iterations; } + /// return values const Values& values() const { return _state().values; } /// @} @@ -215,23 +242,6 @@ public: virtual void iterate() = 0; /// @} - -protected: - - NonlinearFactorGraph graph_; - - /** A default implementation of the optimization loop, which calls iterate() - * until checkConvergence returns true. - */ - void defaultOptimize(); - - virtual const NonlinearOptimizerState& _state() const = 0; - - virtual const NonlinearOptimizerParams& _params() const = 0; - - /** Constructor for initial construction of base classes. */ - NonlinearOptimizer(const NonlinearFactorGraph& graph) : graph_(graph) {} - }; /** Check whether the relative error decrease is less than relativeErrorTreshold,