Changed nonlinear constraints to use boost.bind to handle arbitrary function objects for evaluating cost, useful for parameterizing cost functions.
Alex Cunningham
parent
64b884e722
commit
e26acc0d8d
|
|
@ -1,16 +1,41 @@
|
|||
/*
|
||||
* @file NonlinearConstraint-inl.h
|
||||
* @brief Implementation for NonlinearConstraints
|
||||
* @author alexgc
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/bind.hpp>
|
||||
#include "NonlinearConstraint.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Implementations of base class
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key,
|
||||
size_t dim_lagrange,
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
bool isEquality)
|
||||
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
|
||||
lagrange_key_(lagrange_key), p_(dim_lagrange),
|
||||
isEquality_(isEquality), g_(boost::bind(g, _1, _2)) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key,
|
||||
size_t dim_lagrange,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
bool isEquality)
|
||||
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
|
||||
lagrange_key_(lagrange_key), p_(dim_lagrange),
|
||||
g_(g), isEquality_(isEquality) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
bool NonlinearConstraint<Config>::active(const Config& config) const {
|
||||
|
|
@ -30,13 +55,33 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
|
|||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
|
||||
gradG_(gradG), key_(key) {
|
||||
gradG_(boost::bind(gradG, _1, _2)), key_(key)
|
||||
{
|
||||
// set a good lagrange key here
|
||||
// TODO:should do something smart to find a unique one
|
||||
if (lagrange_key == "")
|
||||
this->lagrange_key_ = "L_" + key;
|
||||
this->keys_.push_front(key);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint1<Config>::NonlinearConstraint1(
|
||||
const std::string& key,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
|
||||
gradG_(gradG), key_(key)
|
||||
{
|
||||
// set a good lagrange key here
|
||||
// TODO:should do something smart to find a unique one
|
||||
if (lagrange_key == "")
|
||||
this->lagrange_key_ = "L_" + key;
|
||||
this->keys_.push_front(key);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
|
|
@ -58,8 +103,6 @@ bool NonlinearConstraint1<Config>::equals(const Factor<Config>& f, double tol) c
|
|||
if (p == NULL) return false;
|
||||
if (key_ != p->key_) return false;
|
||||
if (this->lagrange_key_ != p->lagrange_key_) return false;
|
||||
if (this->g_ != p->g_) return false;
|
||||
if (gradG_ != p->gradG_) return false;
|
||||
if (this->isEquality_ != p->isEquality_) return false;
|
||||
return this->p_ == p->p_;
|
||||
}
|
||||
|
|
@ -104,7 +147,32 @@ NonlinearConstraint2<Config>::NonlinearConstraint2(
|
|||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
|
||||
gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) {
|
||||
gradG1_(boost::bind(gradG1, _1, _2)), gradG2_(boost::bind(gradG2, _1, _2)),
|
||||
key1_(key1), key2_(key2)
|
||||
{
|
||||
// set a good lagrange key here
|
||||
// TODO:should do something smart to find a unique one
|
||||
if (lagrange_key == "")
|
||||
this->lagrange_key_ = "L_" + key1 + key2;
|
||||
this->keys_.push_front(key1);
|
||||
this->keys_.push_back(key2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint2<Config>::NonlinearConstraint2(
|
||||
const std::string& key1,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1,
|
||||
const std::string& key2,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG2,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
|
||||
gradG1_(gradG1), gradG2_(gradG2),
|
||||
key1_(key1), key2_(key2)
|
||||
{
|
||||
// set a good lagrange key here
|
||||
// TODO:should do something smart to find a unique one
|
||||
if (lagrange_key == "")
|
||||
|
|
@ -131,9 +199,6 @@ bool NonlinearConstraint2<Config>::equals(const Factor<Config>& f, double tol) c
|
|||
if (key1_ != p->key1_) return false;
|
||||
if (key2_ != p->key2_) return false;
|
||||
if (this->lagrange_key_ != p->lagrange_key_) return false;
|
||||
if (this->g_ != p->g_) return false;
|
||||
if (gradG1_ != p->gradG1_) return false;
|
||||
if (gradG2_ != p->gradG2_) return false;
|
||||
if (this->isEquality_ != p->isEquality_) return false;
|
||||
return this->p_ == p->p_;
|
||||
}
|
||||
|
|
@ -160,7 +225,7 @@ NonlinearConstraint2<Config>::linearize(const Config& config, const VectorConfig
|
|||
this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
|
||||
|
||||
// construct the constraint
|
||||
GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1*g, 0.0));
|
||||
GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1.0*g, 0.0));
|
||||
|
||||
return std::make_pair(factor, constraint);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <boost/function.hpp>
|
||||
#include "NonlinearFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -37,11 +38,12 @@ protected:
|
|||
|
||||
/** calculates the constraint function of the current config
|
||||
* If the value is zero, the constraint is not active
|
||||
* Use boost.bind to create the function object
|
||||
* @param config is a configuration of all the variables
|
||||
* @param keys is the set of keys - assumed that the function knows how to use
|
||||
* @return the cost for each of p constraints, arranged in a vector
|
||||
*/
|
||||
Vector (*g_)(const Config& config, const std::list<std::string>& keys);
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g_;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -54,10 +56,18 @@ public:
|
|||
NonlinearConstraint(const std::string& lagrange_key,
|
||||
size_t dim_lagrange,
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
bool isEquality=true)
|
||||
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
|
||||
lagrange_key_(lagrange_key), p_(dim_lagrange),
|
||||
isEquality_(isEquality), g_(g) {}
|
||||
bool isEquality=true);
|
||||
|
||||
/** Constructor - sets a more general cost function using boost::bind directly
|
||||
* @param lagrange_key is the label for the associated lagrange multipliers
|
||||
* @param dim_lagrange is the number of associated constraints
|
||||
* @param g is the cost function for the constraint
|
||||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint(const std::string& lagrange_key,
|
||||
size_t dim_lagrange,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
bool isEquality=true);
|
||||
|
||||
/** returns the key used for the Lagrange multipliers */
|
||||
std::string lagrangeKey() const { return lagrange_key_; }
|
||||
|
|
@ -116,11 +126,12 @@ private:
|
|||
/**
|
||||
* Calculates the gradient of the constraint function
|
||||
* returns a pxn matrix
|
||||
* Use boost.bind to create the function object
|
||||
* @param config to use for linearization
|
||||
* @param key of selected variable
|
||||
* @return the jacobian of the constraint in terms of key
|
||||
*/
|
||||
Matrix (*gradG_) (const Config& config, const std::list<std::string>& keys);
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG_;
|
||||
|
||||
/** key for the constrained variable */
|
||||
std::string key_;
|
||||
|
|
@ -144,6 +155,23 @@ public:
|
|||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
||||
/**
|
||||
* Basic constructor with boost function pointers
|
||||
* @param key is the identifier for the variable constrained
|
||||
* @param gradG gives the gradient of the constraint function
|
||||
* @param g is the constraint function as a boost function pointer
|
||||
* @param dim_constraint is the size of the constraint (p)
|
||||
* @param lagrange_key is the identifier for the lagrange multiplier
|
||||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint1(
|
||||
const std::string& key,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
|
|
@ -177,8 +205,8 @@ private:
|
|||
* @param key of selected variable
|
||||
* @return the jacobian of the constraint in terms of key
|
||||
*/
|
||||
Matrix (*gradG1_) (const Config& config, const std::list<std::string>& keys);
|
||||
Matrix (*gradG2_) (const Config& config, const std::list<std::string>& keys);
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1_;
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG2_;
|
||||
|
||||
/** keys for the constrained variables */
|
||||
std::string key1_;
|
||||
|
|
@ -205,6 +233,26 @@ public:
|
|||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
||||
/**
|
||||
* Basic constructor with direct function objects
|
||||
* Use boost.bind to construct the function objects
|
||||
* @param key is the identifier for the variable constrained
|
||||
* @param gradG gives the gradient of the constraint function
|
||||
* @param g is the constraint function
|
||||
* @param dim_constraint is the size of the constraint (p)
|
||||
* @param lagrange_key is the identifier for the lagrange multiplier
|
||||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint2(
|
||||
const std::string& key1,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1,
|
||||
const std::string& key2,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG2,
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <VectorConfig.h>
|
||||
#include <NonlinearConstraint.h>
|
||||
|
|
@ -192,10 +193,7 @@ Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
|
|||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
double g = x*x-5;
|
||||
if (g > 0)
|
||||
return Vector_(1, 0.0); // return exactly zero
|
||||
else
|
||||
return Vector_(1, g); // return the actual cost
|
||||
return Vector_(1, g); // return the actual cost
|
||||
}
|
||||
} // \namespace test1
|
||||
|
||||
|
|
@ -208,14 +206,14 @@ TEST( NonlinearConstraint1, unary_inequality ) {
|
|||
|
||||
// get configurations to use for evaluation
|
||||
VectorConfig config1, config2;
|
||||
config1.insert("x", Vector_(1, 10.0)); // should have zero error
|
||||
config1.insert("x", Vector_(1, 10.0)); // should be inactive
|
||||
config2.insert("x", Vector_(1, 1.0)); // should have nonzero error
|
||||
|
||||
// check error
|
||||
Vector actError1 = c1.error_vector(config1);
|
||||
CHECK(!c1.active(config1));
|
||||
Vector actError2 = c1.error_vector(config2);
|
||||
CHECK(actError1(0) == 0.0); // NOTE: using exact comparison here, as this value is forced
|
||||
CHECK(assert_equal(actError2, Vector_(1, -4.0, 1e-9)));
|
||||
CHECK(c1.active(config2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -252,6 +250,94 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
|
|||
CHECK(assert_equal(*actFactor2, expFactor));
|
||||
CHECK(assert_equal(*actConstraint2, expConstraint));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Binding arbitrary functions
|
||||
/* ************************************************************************* */
|
||||
namespace unary_binding_functions {
|
||||
/** p = 1, gradG(x) = 2*x */
|
||||
Matrix grad_g(double coeff, const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
return Matrix_(1,1, coeff*x);
|
||||
}
|
||||
|
||||
/** p = 1, g(x) x^2 - r > 0 */
|
||||
Vector g_func(double r, const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
double g = x*x-r;
|
||||
return Vector_(1, g); // return the actual cost
|
||||
}
|
||||
} // \namespace unary_binding_functions
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_binding ) {
|
||||
size_t p = 1;
|
||||
double coeff = 2;
|
||||
double radius = 5;
|
||||
NonlinearConstraint1<VectorConfig> c1("x",
|
||||
boost::bind(unary_binding_functions::grad_g, coeff, _1, _2),
|
||||
boost::bind(unary_binding_functions::g_func, radius, _1, _2),
|
||||
p, "L_x1",
|
||||
false); // inequality constraint
|
||||
|
||||
// get configurations to use for evaluation
|
||||
VectorConfig config1, config2;
|
||||
config1.insert("x", Vector_(1, 10.0)); // should have zero error
|
||||
config2.insert("x", Vector_(1, 1.0)); // should have nonzero error
|
||||
|
||||
// check error
|
||||
CHECK(!c1.active(config1));
|
||||
Vector actError2 = c1.error_vector(config2);
|
||||
CHECK(assert_equal(actError2, Vector_(1, -4.0, 1e-9)));
|
||||
CHECK(c1.active(config2));
|
||||
}
|
||||
|
||||
namespace binary_binding_functions {
|
||||
/** gradient for x, gradG(x,y) in x: 2x*/
|
||||
Matrix grad_g1(double c, const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
return Matrix_(1,1, c*x);
|
||||
}
|
||||
|
||||
/** gradient for y, gradG(x,y) in y: -1 */
|
||||
Matrix grad_g2(double c, const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.back()](0);
|
||||
return Matrix_(1,1, -1.0*c);
|
||||
}
|
||||
|
||||
/** p = 1, g(x) = x^2-5 -y = 0 */
|
||||
Vector g_func(double r, const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
double y = config[keys.back()](0);
|
||||
return Vector_(1, x*x-r-y);
|
||||
}
|
||||
} // \namespace test2
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_binding ) {
|
||||
// construct a constraint on x and y
|
||||
// the lagrange multipliers will be expected on L_xy
|
||||
// and there is only one multiplier
|
||||
size_t p = 1;
|
||||
double a = 2.0;
|
||||
double b = 1.0;
|
||||
double r = 5.0;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
"x", boost::bind(binary_binding_functions::grad_g1, a, _1, _2),
|
||||
"y", boost::bind(binary_binding_functions::grad_g2, b, _1, _2),
|
||||
boost::bind(binary_binding_functions::g_func, r, _1, _2), p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig config;
|
||||
config.insert("x", Vector_(1, 1.0));
|
||||
config.insert("y", Vector_(1, 2.0));
|
||||
|
||||
// calculate the error
|
||||
Vector actual = c1.error_vector(config);
|
||||
Vector expected = Vector_(1.0, -6.0);
|
||||
CHECK(assert_equal(actual, expected, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <boost/assign/std/map.hpp> // for insert
|
||||
#include <boost/bind.hpp>
|
||||
#include <Simulated2DMeasurement.h>
|
||||
#include <Simulated2DOdometry.h>
|
||||
#include <simulated2D.h>
|
||||
|
|
@ -348,7 +349,100 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) {
|
|||
CHECK(assert_equal(exp2, *(final.config())));
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
// Use boost bind to parameterize the function
|
||||
namespace sqp_avoid2 {
|
||||
// binary avoidance constraint
|
||||
/** g(x) = ||x2-obs||^2 - radius^2 > 0 */
|
||||
Vector g_func(double radius, const VectorConfig& config, const list<string>& keys) {
|
||||
Vector delta = config[keys.front()]-config[keys.back()];
|
||||
double dist2 = sum(emul(delta, delta));
|
||||
double thresh = radius*radius;
|
||||
return Vector_(1, dist2-thresh);
|
||||
}
|
||||
|
||||
/** gradient at pose */
|
||||
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
|
||||
Vector x2 = config[keys.front()], obs = config[keys.back()];
|
||||
Vector grad = 2.0*(x2-obs);
|
||||
return Matrix_(1,2, grad(0), grad(1));
|
||||
}
|
||||
|
||||
/** gradient at obstacle */
|
||||
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
|
||||
Vector x2 = config[keys.front()], obs = config[keys.back()];
|
||||
Vector grad = -2.0*(x2-obs);
|
||||
return Matrix_(1,2, grad(0), grad(1));
|
||||
}
|
||||
}
|
||||
|
||||
pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
|
||||
// fix start, end, obstacle positions
|
||||
VectorConfig feasible;
|
||||
feasible.insert("x1", Vector_(2, 0.0, 0.0));
|
||||
feasible.insert("x3", Vector_(2, 10.0, 0.0));
|
||||
feasible.insert("obs", Vector_(2, 5.0, -0.5));
|
||||
shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare));
|
||||
shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare));
|
||||
shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare));
|
||||
|
||||
// measurement from x1 to x2
|
||||
Vector x1x2 = Vector_(2, 5.0, 0.0);
|
||||
double sigma1 = 0.1;
|
||||
shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
|
||||
|
||||
// measurement from x2 to x3
|
||||
Vector x2x3 = Vector_(2, 5.0, 0.0);
|
||||
double sigma2 = 0.1;
|
||||
shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
|
||||
|
||||
double radius = 1.0;
|
||||
|
||||
// create a binary inequality constraint that forces the middle point away from
|
||||
// the obstacle
|
||||
shared_NLC2 c1(new NLC2("x2", boost::bind(sqp_avoid2::grad_g1, _1, _2),
|
||||
"obs", boost::bind(sqp_avoid2::grad_g2, _1, _2),
|
||||
boost::bind(sqp_avoid2::g_func, radius, _1, _2), 1, "L_x2obs", false));
|
||||
|
||||
// construct the graph
|
||||
NLGraph graph;
|
||||
graph.push_back(e1);
|
||||
graph.push_back(e2);
|
||||
graph.push_back(e3);
|
||||
graph.push_back(c1);
|
||||
graph.push_back(f1);
|
||||
graph.push_back(f2);
|
||||
|
||||
return make_pair(graph, feasible);
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
TEST ( SQPOptimizer, inequality_avoid_iterative_bind ) {
|
||||
// create the graph
|
||||
NLGraph graph; VectorConfig feasible;
|
||||
boost::tie(graph, feasible) = obstacleAvoidGraphGeneral();
|
||||
|
||||
// create the rest of the config
|
||||
shared_config init(new VectorConfig(feasible));
|
||||
init->insert("x2", Vector_(2, 5.0, 100.0));
|
||||
|
||||
// create an ordering
|
||||
Ordering ord;
|
||||
ord += "x1", "x2", "x3", "obs";
|
||||
|
||||
// create an optimizer
|
||||
Optimizer optimizer(graph, ord, init);
|
||||
|
||||
double relThresh = 1e-5; // minimum change in error between iterations
|
||||
double absThresh = 1e-5; // minimum error necessary to converge
|
||||
double constraintThresh = 1e-9; // minimum constraint error to be feasible
|
||||
Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
|
||||
|
||||
// verify
|
||||
VectorConfig exp2(feasible);
|
||||
exp2.insert("x2", Vector_(2, 5.0, 0.5));
|
||||
CHECK(assert_equal(exp2, *(final.config())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
|
|
|
|||
Loading…
Reference in New Issue