From 872eae951073867543a7db828e17b8f61323a29e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 16 Oct 2024 18:19:33 -0400 Subject: [PATCH 1/9] Rosenbrock function --- ...estNonlinearConjugateGradientOptimizer.cpp | 149 ++++++++++++++++++ 1 file changed, 149 insertions(+) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 36673c7a08..2cd705e88a 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -8,9 +8,12 @@ #include #include +#include #include #include +#include #include +#include #include using namespace std; @@ -79,6 +82,152 @@ TEST(NonlinearConjugateGradientOptimizer, Optimize) { EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } +namespace rosenbrock { + +/// Alias for the first term in the Rosenbrock function +// using Rosenbrock1Factor = PriorFactor; + +using symbol_shorthand::X; +using symbol_shorthand::Y; + +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 sqrt_2 = 1.4142135623730951; + if (H) (*H) = sqrt_2 * Vector::Ones(1).transpose(); + return sqrt_2 * Vector1(x - a_); + } +}; + +/** + * @brief Factor for the second term of the Rosenbrock function. + * f2 = (x*x - y) + * + * 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& p1, const double& p2, + OptionalMatrixType H1, + OptionalMatrixType H2) const override { + double sqrt_2 = 1.4142135623730951; + if (H1) (*H1) = sqrt_2 * p1 * Vector::Ones(1).transpose(); + if (H2) (*H2) = -sqrt_2 * Vector::Ones(1).transpose(); + return sqrt_2 * Vector1((p1 * p1) - p2); + } +}; + +/** + * @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::Unit::Create(1)); + graph.emplace_shared( + X(0), Y(0), noiseModel::Isotropic::Precision(1, 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; + Rosenbrock1Factor f1(X(0), a, noiseModel::Unit::Create(1)); + Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Sigma(1, b)); + Values values; + values.insert(X(0), 0.0); + values.insert(Y(0), 0.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 = 1; + 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 = 16; + // param.verbosity = NonlinearOptimizerParams::LINEAR; + param.verbosity = NonlinearOptimizerParams::SILENT; + + Values initialEstimate; + initialEstimate.insert(X(0), 0.0); + initialEstimate.insert(Y(0), 0.0); + + // std::cout << f(graph, 11.0, 90.0) << std::endl; + + NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); + Values result = optimizer.optimize(); + result.print(); + cout << "cg final error = " << graph.error(result) << endl; +} + /* ************************************************************************* */ int main() { TestResult tr; From 591e1ee4f8aa7f3a3738c737d11fe72cf7ca26bb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 16 Oct 2024 21:31:52 -0400 Subject: [PATCH 2/9] lots of small improvements --- ...estNonlinearConjugateGradientOptimizer.cpp | 41 +++++++++++-------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 2cd705e88a..75aff560fc 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -105,14 +105,15 @@ class Rosenbrock1Factor : public NoiseModelFactorN { /// evaluate error Vector evaluateError(const double& x, OptionalMatrixType H) const override { double sqrt_2 = 1.4142135623730951; - if (H) (*H) = sqrt_2 * Vector::Ones(1).transpose(); - return sqrt_2 * Vector1(x - a_); + double d = a_ - x; + if (H) (*H) = -2 * d * Vector::Ones(1).transpose(); + return Vector1(sqrt_2 * d); } }; /** * @brief Factor for the second term of the Rosenbrock function. - * f2 = (x*x - y) + * f2 = (y - x*x) * * We use the noise model to premultiply with `b`. */ @@ -121,19 +122,22 @@ class Rosenbrock2Factor : public NoiseModelFactorN { typedef Rosenbrock2Factor This; typedef NoiseModelFactorN Base; + double b_; + public: /** Constructor: key1 is x, key2 is y */ - Rosenbrock2Factor(Key key1, Key key2, const SharedNoiseModel& model = nullptr) - : Base(model, key1, key2) {} + Rosenbrock2Factor(Key key1, Key key2, double b, + const SharedNoiseModel& model = nullptr) + : Base(model, key1, key2), b_(b) {} /// evaluate error - Vector evaluateError(const double& p1, const double& p2, - OptionalMatrixType H1, + Vector evaluateError(const double& x, const double& y, OptionalMatrixType H1, OptionalMatrixType H2) const override { double sqrt_2 = 1.4142135623730951; - if (H1) (*H1) = sqrt_2 * p1 * Vector::Ones(1).transpose(); - if (H2) (*H2) = -sqrt_2 * Vector::Ones(1).transpose(); - return sqrt_2 * Vector1((p1 * p1) - p2); + double x2 = x * x, d = y - x2; + if (H1) (*H1) = -2 * b_ * d * 2 * x * Vector::Ones(1).transpose(); + if (H2) (*H2) = 2 * b_ * d * Vector::Ones(1).transpose(); + return Vector1(sqrt_2 * d); } }; @@ -152,7 +156,7 @@ static NonlinearFactorGraph GetRosenbrockGraph(double a = 1.0, NonlinearFactorGraph graph; graph.emplace_shared(X(0), a, noiseModel::Unit::Create(1)); graph.emplace_shared( - X(0), Y(0), noiseModel::Isotropic::Precision(1, b)); + X(0), Y(0), b, noiseModel::Isotropic::Precision(1, b)); return graph; } @@ -181,12 +185,12 @@ TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { using namespace rosenbrock; double a = 1.0, b = 100.0; Rosenbrock1Factor f1(X(0), a, noiseModel::Unit::Create(1)); - Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Sigma(1, b)); + Rosenbrock2Factor f2(X(0), Y(0), b, noiseModel::Isotropic::Sigma(1, b)); Values values; values.insert(X(0), 0.0); values.insert(Y(0), 0.0); - EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-7, 1e-5); + // 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); @@ -213,13 +217,18 @@ TEST(NonlinearConjugateGradientOptimizer, Optimization) { NonlinearOptimizerParams param; param.maxIterations = 16; - // param.verbosity = NonlinearOptimizerParams::LINEAR; - param.verbosity = NonlinearOptimizerParams::SILENT; + param.verbosity = NonlinearOptimizerParams::LINEAR; + // param.verbosity = NonlinearOptimizerParams::SILENT; Values initialEstimate; initialEstimate.insert(X(0), 0.0); initialEstimate.insert(Y(0), 0.0); + // std::cout << f(graph, 0, 0) << std::endl; + GaussianFactorGraph::shared_ptr linear = graph.linearize(initialEstimate); + linear->print(); + linear->gradientAtZero().print("Gradient: "); + // std::cout << f(graph, 11.0, 90.0) << std::endl; NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); From 83e3fdaa2488fd0e8a8281c09abf7654a79dccb8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Oct 2024 10:22:49 -0400 Subject: [PATCH 3/9] Rosenbrock function implemented as factor graph --- ...estNonlinearConjugateGradientOptimizer.cpp | 50 +++++++++---------- 1 file changed, 24 insertions(+), 26 deletions(-) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 75aff560fc..1e16c65090 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -90,6 +90,8 @@ namespace rosenbrock { using symbol_shorthand::X; using symbol_shorthand::Y; +constexpr double sqrt_2 = 1.4142135623730951; + class Rosenbrock1Factor : public NoiseModelFactorN { private: typedef Rosenbrock1Factor This; @@ -104,9 +106,9 @@ class Rosenbrock1Factor : public NoiseModelFactorN { /// evaluate error Vector evaluateError(const double& x, OptionalMatrixType H) const override { - double sqrt_2 = 1.4142135623730951; - double d = a_ - x; - if (H) (*H) = -2 * d * Vector::Ones(1).transpose(); + double d = x - a_; + // Because linearized gradient is -A'b, it will multiply by d + if (H) (*H) = Vector1(2 / sqrt_2).transpose(); return Vector1(sqrt_2 * d); } }; @@ -122,21 +124,18 @@ class Rosenbrock2Factor : public NoiseModelFactorN { typedef Rosenbrock2Factor This; typedef NoiseModelFactorN Base; - double b_; - public: /** Constructor: key1 is x, key2 is y */ - Rosenbrock2Factor(Key key1, Key key2, double b, - const SharedNoiseModel& model = nullptr) - : Base(model, key1, key2), b_(b) {} + 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 sqrt_2 = 1.4142135623730951; - double x2 = x * x, d = y - x2; - if (H1) (*H1) = -2 * b_ * d * 2 * x * Vector::Ones(1).transpose(); - if (H2) (*H2) = 2 * b_ * d * Vector::Ones(1).transpose(); + double x2 = x * x, d = x2 - y; + // Because linearized gradient is -A'b, it will multiply by d + if (H1) (*H1) = Vector1(4 * x / sqrt_2).transpose(); + if (H2) (*H2) = Vector1(-2 / sqrt_2).transpose(); return Vector1(sqrt_2 * d); } }; @@ -156,7 +155,7 @@ static NonlinearFactorGraph GetRosenbrockGraph(double a = 1.0, NonlinearFactorGraph graph; graph.emplace_shared(X(0), a, noiseModel::Unit::Create(1)); graph.emplace_shared( - X(0), Y(0), b, noiseModel::Isotropic::Precision(1, b)); + X(0), Y(0), noiseModel::Isotropic::Precision(1, b)); return graph; } @@ -185,12 +184,12 @@ TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { using namespace rosenbrock; double a = 1.0, b = 100.0; Rosenbrock1Factor f1(X(0), a, noiseModel::Unit::Create(1)); - Rosenbrock2Factor f2(X(0), Y(0), b, noiseModel::Isotropic::Sigma(1, b)); + Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Sigma(1, b)); Values values; values.insert(X(0), 0.0); values.insert(Y(0), 0.0); - // EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-7, 1e-5); - // EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-7, 1e-5); + 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); @@ -208,7 +207,7 @@ TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { TEST(NonlinearConjugateGradientOptimizer, Optimization) { using namespace rosenbrock; - double a = 1; + double a = 7; auto graph = GetRosenbrockGraph(a); // Assert that error at global minimum is 0. @@ -216,21 +215,20 @@ TEST(NonlinearConjugateGradientOptimizer, Optimization) { EXPECT_DOUBLES_EQUAL(0.0, error, 1e-12); NonlinearOptimizerParams param; - param.maxIterations = 16; - param.verbosity = NonlinearOptimizerParams::LINEAR; - // param.verbosity = NonlinearOptimizerParams::SILENT; + param.maxIterations = 50; + // param.verbosity = NonlinearOptimizerParams::LINEAR; + param.verbosity = NonlinearOptimizerParams::SILENT; + double x = 3.0, y = 5.0; Values initialEstimate; - initialEstimate.insert(X(0), 0.0); - initialEstimate.insert(Y(0), 0.0); + initialEstimate.insert(X(0), x); + initialEstimate.insert(Y(0), y); - // std::cout << f(graph, 0, 0) << std::endl; + std::cout << f(graph, x, y) << std::endl; GaussianFactorGraph::shared_ptr linear = graph.linearize(initialEstimate); - linear->print(); + // linear->print(); linear->gradientAtZero().print("Gradient: "); - // std::cout << f(graph, 11.0, 90.0) << std::endl; - NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); result.print(); From 8a0bbe9666ef486d91a5d8b2f39080937e3e6188 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Oct 2024 13:28:06 -0400 Subject: [PATCH 4/9] test optimization with Rosenbrock function --- .../testNonlinearConjugateGradientOptimizer.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 1e16c65090..76edd37564 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -207,7 +207,7 @@ TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { TEST(NonlinearConjugateGradientOptimizer, Optimization) { using namespace rosenbrock; - double a = 7; + double a = 12; auto graph = GetRosenbrockGraph(a); // Assert that error at global minimum is 0. @@ -215,7 +215,7 @@ TEST(NonlinearConjugateGradientOptimizer, Optimization) { EXPECT_DOUBLES_EQUAL(0.0, error, 1e-12); NonlinearOptimizerParams param; - param.maxIterations = 50; + param.maxIterations = 350; // param.verbosity = NonlinearOptimizerParams::LINEAR; param.verbosity = NonlinearOptimizerParams::SILENT; @@ -224,15 +224,20 @@ TEST(NonlinearConjugateGradientOptimizer, Optimization) { initialEstimate.insert(X(0), x); initialEstimate.insert(Y(0), y); - std::cout << f(graph, x, y) << std::endl; GaussianFactorGraph::shared_ptr linear = graph.linearize(initialEstimate); + // std::cout << "error: " << f(graph, x, y) << std::endl; // linear->print(); - linear->gradientAtZero().print("Gradient: "); + // linear->gradientAtZero().print("Gradient: "); NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); - result.print(); - cout << "cg final error = " << graph.error(result) << endl; + // 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)); } /* ************************************************************************* */ From 631fa2b564ca703e5e6887955a443da59c75602f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Oct 2024 13:38:11 -0400 Subject: [PATCH 5/9] minor improvements --- .../tests/testNonlinearConjugateGradientOptimizer.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 76edd37564..9681caff0b 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -19,6 +19,9 @@ 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 @@ -84,12 +87,6 @@ TEST(NonlinearConjugateGradientOptimizer, Optimize) { namespace rosenbrock { -/// Alias for the first term in the Rosenbrock function -// using Rosenbrock1Factor = PriorFactor; - -using symbol_shorthand::X; -using symbol_shorthand::Y; - constexpr double sqrt_2 = 1.4142135623730951; class Rosenbrock1Factor : public NoiseModelFactorN { From 58ae5c6d08decc28ab7466a89de96567ede6c3bc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Oct 2024 18:27:39 -0400 Subject: [PATCH 6/9] fix test --- .../nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 9681caff0b..4dfa7d9125 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -181,7 +181,7 @@ TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { using namespace rosenbrock; double a = 1.0, b = 100.0; Rosenbrock1Factor f1(X(0), a, noiseModel::Unit::Create(1)); - Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Sigma(1, b)); + Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Precision(1, b)); Values values; values.insert(X(0), 0.0); values.insert(Y(0), 0.0); From b5b5e1544398acebd9ff1c963d078e8eec4d5950 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Oct 2024 18:48:53 -0400 Subject: [PATCH 7/9] move 2 to the Precision matrix and check if error is correct --- ...estNonlinearConjugateGradientOptimizer.cpp | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 4dfa7d9125..ad6cc76bf9 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -69,7 +69,7 @@ std::tuple generateProblem() { } /* ************************************************************************* */ -TEST(NonlinearConjugateGradientOptimizer, Optimize) { +TEST_DISABLED(NonlinearConjugateGradientOptimizer, Optimize) { const auto [graph, initialEstimate] = generateProblem(); // cout << "initial error = " << graph.error(initialEstimate) << endl; @@ -106,7 +106,7 @@ class Rosenbrock1Factor : public NoiseModelFactorN { double d = x - a_; // Because linearized gradient is -A'b, it will multiply by d if (H) (*H) = Vector1(2 / sqrt_2).transpose(); - return Vector1(sqrt_2 * d); + return Vector1(d); } }; @@ -133,7 +133,7 @@ class Rosenbrock2Factor : public NoiseModelFactorN { // Because linearized gradient is -A'b, it will multiply by d if (H1) (*H1) = Vector1(4 * x / sqrt_2).transpose(); if (H2) (*H2) = Vector1(-2 / sqrt_2).transpose(); - return Vector1(sqrt_2 * d); + return Vector1(d); } }; @@ -150,9 +150,10 @@ class Rosenbrock2Factor : public NoiseModelFactorN { static NonlinearFactorGraph GetRosenbrockGraph(double a = 1.0, double b = 100.0) { NonlinearFactorGraph graph; - graph.emplace_shared(X(0), a, noiseModel::Unit::Create(1)); + graph.emplace_shared( + X(0), a, noiseModel::Isotropic::Precision(1, 2)); graph.emplace_shared( - X(0), Y(0), noiseModel::Isotropic::Precision(1, b)); + X(0), Y(0), noiseModel::Isotropic::Precision(1, 2 * b)); return graph; } @@ -180,13 +181,13 @@ double rosenbrock_func(double x, double y, double a = 1.0, double b = 100.0) { TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { using namespace rosenbrock; double a = 1.0, b = 100.0; - Rosenbrock1Factor f1(X(0), a, noiseModel::Unit::Create(1)); - Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Precision(1, b)); + Rosenbrock1Factor f1(X(0), a, noiseModel::Isotropic::Precision(1, 2)); + Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Precision(1, 2 * b)); Values values; - values.insert(X(0), 0.0); - values.insert(Y(0), 0.0); - EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-7, 1e-5); + 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); @@ -201,7 +202,7 @@ TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { /* ************************************************************************* */ // Optimize the Rosenbrock function to verify optimizer works -TEST(NonlinearConjugateGradientOptimizer, Optimization) { +TEST_DISABLED(NonlinearConjugateGradientOptimizer, Optimization) { using namespace rosenbrock; double a = 12; From 6d6d39287e96aaa10704a00376611a9f0b588ab0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Oct 2024 18:57:32 -0400 Subject: [PATCH 8/9] fix jacobians --- ...estNonlinearConjugateGradientOptimizer.cpp | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index ad6cc76bf9..1742dc0cb7 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -69,7 +69,7 @@ std::tuple generateProblem() { } /* ************************************************************************* */ -TEST_DISABLED(NonlinearConjugateGradientOptimizer, Optimize) { +TEST(NonlinearConjugateGradientOptimizer, Optimize) { const auto [graph, initialEstimate] = generateProblem(); // cout << "initial error = " << graph.error(initialEstimate) << endl; @@ -104,8 +104,8 @@ class Rosenbrock1Factor : public NoiseModelFactorN { /// evaluate error Vector evaluateError(const double& x, OptionalMatrixType H) const override { double d = x - a_; - // Because linearized gradient is -A'b, it will multiply by d - if (H) (*H) = Vector1(2 / sqrt_2).transpose(); + // Because linearized gradient is -A'b/sigma, it will multiply by d + if (H) (*H) = Vector1(1).transpose(); return Vector1(d); } }; @@ -130,9 +130,9 @@ class Rosenbrock2Factor : public NoiseModelFactorN { 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, it will multiply by d - if (H1) (*H1) = Vector1(4 * x / sqrt_2).transpose(); - if (H2) (*H2) = Vector1(-2 / sqrt_2).transpose(); + // 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); } }; @@ -181,13 +181,16 @@ double rosenbrock_func(double x, double y, double a = 1.0, double b = 100.0) { TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { using namespace rosenbrock; double a = 1.0, b = 100.0; - Rosenbrock1Factor f1(X(0), a, noiseModel::Isotropic::Precision(1, 2)); - Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Precision(1, 2 * b)); + 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); + 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); @@ -202,7 +205,7 @@ TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) { /* ************************************************************************* */ // Optimize the Rosenbrock function to verify optimizer works -TEST_DISABLED(NonlinearConjugateGradientOptimizer, Optimization) { +TEST(NonlinearConjugateGradientOptimizer, Optimization) { using namespace rosenbrock; double a = 12; From 893e69df2906a8abcb0bdfd7842d0bdb574926a2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Oct 2024 18:57:48 -0400 Subject: [PATCH 9/9] remove sqrt definition --- .../nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp index 1742dc0cb7..bd1febcc6b 100644 --- a/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp @@ -87,8 +87,6 @@ TEST(NonlinearConjugateGradientOptimizer, Optimize) { namespace rosenbrock { -constexpr double sqrt_2 = 1.4142135623730951; - class Rosenbrock1Factor : public NoiseModelFactorN { private: typedef Rosenbrock1Factor This;