Rosenbrock function
parent
2d3a296d06
commit
872eae9510
|
@ -8,9 +8,12 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
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<double>;
|
||||
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::Y;
|
||||
|
||||
class Rosenbrock1Factor : public NoiseModelFactorN<double> {
|
||||
private:
|
||||
typedef Rosenbrock1Factor This;
|
||||
typedef NoiseModelFactorN<double> 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<double, double> {
|
||||
private:
|
||||
typedef Rosenbrock2Factor This;
|
||||
typedef NoiseModelFactorN<double, double> 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<Rosenbrock1Factor>(X(0), a, noiseModel::Unit::Create(1));
|
||||
graph.emplace_shared<Rosenbrock2Factor>(
|
||||
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<double>(X(0), x);
|
||||
initial.insert<double>(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<double>(X(0), 0.0);
|
||||
values.insert<double>(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<double> 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<double>(X(0), 0.0);
|
||||
initialEstimate.insert<double>(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;
|
||||
|
|
Loading…
Reference in New Issue