89 lines
2.7 KiB
C++
89 lines
2.7 KiB
C++
/*
|
|
* @file testNonlinearConstraint.cpp
|
|
* @brief Tests for nonlinear constraints handled via SQP
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <VectorConfig.h>
|
|
#include <NonlinearConstraint.h>
|
|
|
|
using namespace gtsam;
|
|
|
|
// define some functions to use
|
|
// these examples use scalar variables and a single constraint
|
|
|
|
/* ************************************************************************* */
|
|
// unary functions
|
|
/* ************************************************************************* */
|
|
namespace test1 {
|
|
/** p = 1, gradG(x) = 2x */
|
|
Matrix grad_g(const VectorConfig& config, const std::string& key) {
|
|
double x = config[key](0);
|
|
return Matrix_(1,1, 2*x);
|
|
}
|
|
|
|
/** p = 1, g(x) = x^2-5 = 0 */
|
|
Vector g_func(const VectorConfig& config, const std::string& key) {
|
|
double x = config[key](0);
|
|
return Vector_(1, x*x-5);
|
|
}
|
|
} // \namespace test1
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearConstraint, unary_construction ) {
|
|
// construct a constraint on x
|
|
// the lagrange multipliers will be expected on L_x1
|
|
// and there is only one multiplier
|
|
size_t p = 1;
|
|
NonlinearConstraint1<VectorConfig> c1("x", *test1::grad_g, *test1::g_func, p, "L_x1");
|
|
|
|
// get a configuration to use for finding the error
|
|
VectorConfig config;
|
|
config.insert("x", Vector_(1, 1.0));
|
|
|
|
// calculate the error
|
|
Vector actual = c1.error_vector(config);
|
|
Vector expected = Vector_(1.0, -4.0);
|
|
CHECK(assert_equal(actual, expected, 1e-5));
|
|
}
|
|
|
|
///* ************************************************************************* */
|
|
//TEST( NonlinearConstraint, unary_linearize ) {
|
|
// // constuct a constraint
|
|
// Vector lambda = Vector_(1, 1.0);
|
|
// NonlinearConstraint1<VectorConfig> c1("x", *grad_g1, *g1_func, lambda);
|
|
//
|
|
// // get a configuration to use for linearization
|
|
// VectorConfig config;
|
|
// config.insert("x", Vector_(1, 1.0));
|
|
//
|
|
// // determine the gradient of the function
|
|
// Matrix
|
|
//
|
|
//}
|
|
|
|
///* ************************************************************************* */
|
|
//// binary functions
|
|
//Matrix grad1_g2(const VectorConfig& config) {
|
|
// return eye(1);
|
|
//}
|
|
//
|
|
//Matrix grad2_g2(const VectorConfig& config) {
|
|
// return eye(1);
|
|
//}
|
|
//
|
|
//Vector g2_func(const VectorConfig& config) {
|
|
// return zero(1);
|
|
//}
|
|
//
|
|
///* ************************************************************************* */
|
|
//TEST( NonlinearConstraint, binary_construction ) {
|
|
// Vector lambda = Vector_(1, 1.0);
|
|
// NonlinearConstraint2<VectorConfig> c1("x", *grad1_g2, "y", *grad2_g2, *g2_func, lambda);
|
|
//}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|