diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index f216e4a8c..970036105 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -8,14 +8,20 @@ #include #include +#include #include #include +#include #include +#include #include using namespace std; using namespace gtsam; +using symbol_shorthand::X; +using symbol_shorthand::Y; + // Generate a small PoseSLAM problem std::tuple generateProblem() { // 1. Create graph container and add factors to it @@ -79,6 +85,160 @@ TEST(NonlinearConjugateGradientOptimizer, Optimize) { EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } +namespace rosenbrock { + +class Rosenbrock1Factor : public NoiseModelFactorN { + private: + typedef Rosenbrock1Factor This; + typedef NoiseModelFactorN Base; + + double a_; + + public: + /** Constructor: key is x */ + Rosenbrock1Factor(Key key, double a, const SharedNoiseModel& model = nullptr) + : Base(model, key), a_(a) {} + + /// evaluate error + Vector evaluateError(const double& x, OptionalMatrixType H) const override { + double d = x - a_; + // Because linearized gradient is -A'b/sigma, it will multiply by d + if (H) (*H) = Vector1(1).transpose(); + return Vector1(d); + } +}; + +/** + * @brief Factor for the second term of the Rosenbrock function. + * f2 = (y - x*x) + * + * We use the noise model to premultiply with `b`. + */ +class Rosenbrock2Factor : public NoiseModelFactorN { + private: + typedef Rosenbrock2Factor This; + typedef NoiseModelFactorN Base; + + public: + /** Constructor: key1 is x, key2 is y */ + Rosenbrock2Factor(Key key1, Key key2, const SharedNoiseModel& model = nullptr) + : Base(model, key1, key2) {} + + /// evaluate error + Vector evaluateError(const double& x, const double& y, OptionalMatrixType H1, + OptionalMatrixType H2) const override { + double x2 = x * x, d = x2 - y; + // Because linearized gradient is -A'b/sigma, it will multiply by d + if (H1) (*H1) = Vector1(2 * x).transpose(); + if (H2) (*H2) = Vector1(-1).transpose(); + return Vector1(d); + } +}; + +/** + * @brief Get a nonlinear factor graph representing + * the Rosenbrock Banana function. + * + * More details: https://en.wikipedia.org/wiki/Rosenbrock_function + * + * @param a + * @param b + * @return NonlinearFactorGraph + */ +static NonlinearFactorGraph GetRosenbrockGraph(double a = 1.0, + double b = 100.0) { + NonlinearFactorGraph graph; + graph.emplace_shared( + X(0), a, noiseModel::Isotropic::Precision(1, 2)); + graph.emplace_shared( + X(0), Y(0), noiseModel::Isotropic::Precision(1, 2 * b)); + + return graph; +} + +/// Compute the Rosenbrock function error given the nonlinear factor graph +/// and input values. +double f(const NonlinearFactorGraph& graph, double x, double y) { + Values initial; + initial.insert(X(0), x); + initial.insert(Y(0), y); + + return graph.error(initial); +} + +/// True Rosenbrock Banana function. +double rosenbrock_func(double x, double y, double a = 1.0, double b = 100.0) { + double m = (a - x) * (a - x); + double n = b * (y - x * x) * (y - x * x); + return m + n; +} +} // namespace rosenbrock + +/* ************************************************************************* */ +// Test whether the 2 factors are properly implemented. +TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { + using namespace rosenbrock; + double a = 1.0, b = 100.0; + auto graph = GetRosenbrockGraph(a, b); + Rosenbrock1Factor f1 = + *std::static_pointer_cast(graph.at(0)); + Rosenbrock2Factor f2 = + *std::static_pointer_cast(graph.at(1)); + Values values; + values.insert(X(0), 3.0); + values.insert(Y(0), 5.0); + EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-7, 1e-5); + + std::mt19937 rng(42); + std::uniform_real_distribution dist(0.0, 100.0); + for (size_t i = 0; i < 50; ++i) { + double x = dist(rng); + double y = dist(rng); + + auto graph = GetRosenbrockGraph(a, b); + EXPECT_DOUBLES_EQUAL(rosenbrock_func(x, y, a, b), f(graph, x, y), 1e-5); + } +} + +/* ************************************************************************* */ +// Optimize the Rosenbrock function to verify optimizer works +TEST(NonlinearConjugateGradientOptimizer, Optimization) { + using namespace rosenbrock; + + double a = 12; + auto graph = GetRosenbrockGraph(a); + + // Assert that error at global minimum is 0. + double error = f(graph, a, a * a); + EXPECT_DOUBLES_EQUAL(0.0, error, 1e-12); + + NonlinearOptimizerParams param; + param.maxIterations = 350; + // param.verbosity = NonlinearOptimizerParams::LINEAR; + param.verbosity = NonlinearOptimizerParams::SILENT; + + double x = 3.0, y = 5.0; + Values initialEstimate; + initialEstimate.insert(X(0), x); + initialEstimate.insert(Y(0), y); + + GaussianFactorGraph::shared_ptr linear = graph.linearize(initialEstimate); + // std::cout << "error: " << f(graph, x, y) << std::endl; + // linear->print(); + // linear->gradientAtZero().print("Gradient: "); + + NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); + Values result = optimizer.optimize(); + // result.print(); + // cout << "cg final error = " << graph.error(result) << endl; + + Values expected; + expected.insert(X(0), a); + expected.insert(Y(0), a * a); + EXPECT(assert_equal(expected, result, 1e-1)); +} + /* ************************************************************************* */ /// Test different direction methods TEST(NonlinearConjugateGradientOptimizer, DirectionMethods) {