12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix jacobians

release/4.3a0
Varun Agrawal 2024-10-17 18:57:32 -04:00
parent b5b5e15443
commit 6d6d39287e
1 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/nonlinear/tests/testNonlinearConjugateGradientOptimizer.cpp
View File

@ -69,7 +69,7 @@ std::tuple<NonlinearFactorGraph, Values> generateProblem() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST_DISABLED(NonlinearConjugateGradientOptimizer, Optimize) { TEST(NonlinearConjugateGradientOptimizer, Optimize) {
const auto [graph, initialEstimate] = generateProblem(); const auto [graph, initialEstimate] = generateProblem();
// cout << "initial error = " << graph.error(initialEstimate) << endl; // cout << "initial error = " << graph.error(initialEstimate) << endl;
@ -104,8 +104,8 @@ class Rosenbrock1Factor : public NoiseModelFactorN<double> {
/// evaluate error /// evaluate error
Vector evaluateError(const double& x, OptionalMatrixType H) const override { Vector evaluateError(const double& x, OptionalMatrixType H) const override {
double d = x - a_; double d = x - a_;
// Because linearized gradient is -A'b, it will multiply by d // Because linearized gradient is -A'b/sigma, it will multiply by d
if (H) (*H) = Vector1(2 / sqrt_2).transpose(); if (H) (*H) = Vector1(1).transpose();
return Vector1(d); return Vector1(d);
} }
}; };
@ -130,9 +130,9 @@ class Rosenbrock2Factor : public NoiseModelFactorN<double, double> {
Vector evaluateError(const double& x, const double& y, OptionalMatrixType H1, Vector evaluateError(const double& x, const double& y, OptionalMatrixType H1,
OptionalMatrixType H2) const override { OptionalMatrixType H2) const override {
double x2 = x * x, d = x2 - y; double x2 = x * x, d = x2 - y;
// Because linearized gradient is -A'b, it will multiply by d // Because linearized gradient is -A'b/sigma, it will multiply by d
if (H1) (*H1) = Vector1(4 * x / sqrt_2).transpose(); if (H1) (*H1) = Vector1(2 * x).transpose();
if (H2) (*H2) = Vector1(-2 / sqrt_2).transpose(); if (H2) (*H2) = Vector1(-1).transpose();
return Vector1(d); 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) { TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) {
using namespace rosenbrock; using namespace rosenbrock;
double a = 1.0, b = 100.0; double a = 1.0, b = 100.0;
Rosenbrock1Factor f1(X(0), a, noiseModel::Isotropic::Precision(1, 2)); auto graph = GetRosenbrockGraph(a, b);
Rosenbrock2Factor f2(X(0), Y(0), noiseModel::Isotropic::Precision(1, 2 * b)); Rosenbrock1Factor f1 =
*std::static_pointer_cast<Rosenbrock1Factor>(graph.at(0));
Rosenbrock2Factor f2 =
*std::static_pointer_cast<Rosenbrock2Factor>(graph.at(1));
Values values; Values values;
values.insert<double>(X(0), 3.0); values.insert<double>(X(0), 3.0);
values.insert<double>(Y(0), 5.0); values.insert<double>(Y(0), 5.0);
// EXPECT_CORRECT_FACTOR_JACOBIANS(f1, 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); EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-7, 1e-5);
std::mt19937 rng(42); std::mt19937 rng(42);
std::uniform_real_distribution<double> dist(0.0, 100.0); std::uniform_real_distribution<double> dist(0.0, 100.0);
@ -202,7 +205,7 @@ TEST(NonlinearConjugateGradientOptimizer, Rosenbrock) {
/* ************************************************************************* */ /* ************************************************************************* */
// Optimize the Rosenbrock function to verify optimizer works // Optimize the Rosenbrock function to verify optimizer works
TEST_DISABLED(NonlinearConjugateGradientOptimizer, Optimization) { TEST(NonlinearConjugateGradientOptimizer, Optimization) {
using namespace rosenbrock; using namespace rosenbrock;
double a = 12; double a = 12;