Added a NonlinearConstraint and testNonlinearConstraint. There is currently an abstract base class for constraints and a partially implemented unary constraint.
Alex Cunningham
parent
cd913566f2
commit
429f27550c
|
|
@ -521,10 +521,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testIncremental.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testIncremental.run</buildTarget>
|
||||
<buildTarget>testNonlinearConstraint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
|
|||
|
|
@ -115,9 +115,12 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
|
|||
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
|
||||
sources += NonlinearFactor.cpp
|
||||
sources += NonlinearEquality.h
|
||||
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP
|
||||
sources += NonlinearConstraint.h
|
||||
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint
|
||||
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
|
||||
testNonlinearFactor_LDADD = libgtsam.la
|
||||
testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp
|
||||
testNonlinearConstraint_LDADD = libgtsam.la
|
||||
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
|
||||
testNonlinearFactorGraph_LDADD = libgtsam.la
|
||||
testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,224 @@
|
|||
/*
|
||||
* @file NonlinearConstraint.h
|
||||
* @brief Implements nonlinear constraints that can be linearized and
|
||||
* inserted into an existing nonlinear graph and solved via SQP
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include "NonlinearFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Base class for nonlinear constraints
|
||||
* This allows for both equality and inequality constraints,
|
||||
* where equality constraints are active all the time (even slightly
|
||||
* nonzero constraint functions will still be active - inequality
|
||||
* constraints should be sure to force to actual zero)
|
||||
*
|
||||
* The measurement z in the underlying NonlinearFactor is the
|
||||
* set of Lagrange multipliers.
|
||||
*/
|
||||
template <class Config>
|
||||
class NonlinearConstraint : public NonlinearFactor<Config> {
|
||||
|
||||
protected:
|
||||
/** key for the lagrange multipliers */
|
||||
std::string lagrange_key_;
|
||||
|
||||
/** number of lagrange multipliers */
|
||||
size_t p_;
|
||||
|
||||
public:
|
||||
/** Constructor - sets the cost function and the lagrange multipliers
|
||||
* @param lagrange_key is the label for the associated lagrange multipliers
|
||||
* @param dim_lagrange is the number of associated constraints
|
||||
*/
|
||||
NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange) :
|
||||
NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
|
||||
lagrange_key_(lagrange_key), p_(dim_lagrange) {}
|
||||
|
||||
/** returns the key used for the Lagrange multipliers */
|
||||
std::string& lagrangeKey() const { return lagrange_key_; }
|
||||
|
||||
/** returns the number of lagrange multipliers */
|
||||
size_t nrConstraints() const { return p_; }
|
||||
|
||||
/** Print */
|
||||
virtual void print(const std::string& s = "") const =0;
|
||||
|
||||
/** Check if two factors are equal */
|
||||
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
|
||||
|
||||
/** error function - returns the result of the constraint function */
|
||||
virtual inline Vector error_vector(const Config& c) const=0;
|
||||
|
||||
/**
|
||||
* Linearize using a real Config and a VectorConfig of Lagrange multipliers
|
||||
* Returns the two separate Gaussian factors to solve
|
||||
* @param config is the real Config of the real variables
|
||||
* @param lagrange is the VectorConfig of lagrange multipliers
|
||||
* @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
|
||||
*/
|
||||
virtual std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
|
||||
linearize(const Config& config, const VectorConfig& lagrange) const=0;
|
||||
|
||||
/**
|
||||
* linearize with only Config, which is not currently implemented
|
||||
* This will be implemented later for other constrained optimization
|
||||
* algorithms
|
||||
*/
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
|
||||
throw std::invalid_argument("No current constraint linearization for a single Config!");
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* A unary constraint with arbitrary cost and gradient functions
|
||||
*/
|
||||
template <class Config>
|
||||
class NonlinearConstraint1 : NonlinearConstraint<Config> {
|
||||
|
||||
private:
|
||||
/** calculates the constraint function of the current config
|
||||
* If the value is zero, the constraint is not active
|
||||
* @param config is a configuration of all the variables
|
||||
* @param key is the id for the selected variable
|
||||
* @return the cost for each of p constraints, arranged in a vector
|
||||
*/
|
||||
Vector (*g_)(const Config& config, const std::string& key);
|
||||
|
||||
/**
|
||||
* Calculates the gradient of the constraint function
|
||||
* returns a pxn matrix
|
||||
* @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::string& key);
|
||||
|
||||
/** key for the constrained variable */
|
||||
std::string key_;
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Basic constructor
|
||||
* @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
|
||||
*/
|
||||
NonlinearConstraint1(
|
||||
const std::string& key,
|
||||
Matrix (*gradG)(const Config& config, const std::string& key),
|
||||
Vector (*g)(const Config& config, const std::string& key),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="") :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint),
|
||||
g_(g), gradG_(gradG), key_(key) {
|
||||
// set a good lagrange key here - should do something smart to find a unique one
|
||||
if (lagrange_key == "")
|
||||
this->lagrange_key_ = "L_" + key;
|
||||
}
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "") const {
|
||||
//FIXME: dummy implementation
|
||||
}
|
||||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const Factor<Config>& f, double tol=1e-9) const {
|
||||
//FIXME: dummy implementation
|
||||
return false;
|
||||
}
|
||||
|
||||
/** error function - returns the result of the constraint function */
|
||||
inline Vector error_vector(const Config& c) const {
|
||||
return g_(c, key_);
|
||||
}
|
||||
|
||||
/**
|
||||
* Linearize using a real Config and a VectorConfig of Lagrange multipliers
|
||||
* Returns the two separate Gaussian factors to solve
|
||||
* @param config is the real Config of the real variables
|
||||
* @param lagrange is the VectorConfig of lagrange multipliers
|
||||
* @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
|
||||
*/
|
||||
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
|
||||
linearize(const Config& config, const VectorConfig& lagrange) const {
|
||||
//FIXME: dummy implementation
|
||||
GaussianFactor::shared_ptr factor(new GaussianFactor);
|
||||
GaussianFactor::shared_ptr constraint(new GaussianFactor);
|
||||
return std::make_pair(factor, constraint);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
//template <class Config>
|
||||
//class NonlinearConstraint2 : public NonlinearConstraint<Config> {
|
||||
//
|
||||
//private:
|
||||
// /**
|
||||
// * Calculates the gradient of the constraint function
|
||||
// * returns a pxn matrix for x1
|
||||
// * @param config
|
||||
// */
|
||||
// Matrix (*gradG1_) (const Config& config);
|
||||
//
|
||||
// /**
|
||||
// * Calculates the gradient of the constraint function
|
||||
// * returns a pxn matrix for x2
|
||||
// * @param config
|
||||
// */
|
||||
// Matrix (*gradG2_) (const Config& config);
|
||||
//
|
||||
// /** keys for the constrained variables */
|
||||
// std::string key1_;
|
||||
// std::string key2_;
|
||||
//
|
||||
//public:
|
||||
//
|
||||
// /**
|
||||
// * Basic constructor
|
||||
// * @param key1 is the first variable
|
||||
// * @param gradG1 gives the gradient for the first variable
|
||||
// * @param key2 is the first variable
|
||||
// * @param gradG2 gives the gradient for the first variable
|
||||
// * @param g is the constraint function
|
||||
// * @param lambdas is vector of size p with Lagrange multipliers
|
||||
// */
|
||||
// NonlinearConstraint2(
|
||||
// const std::string& key1,
|
||||
// Matrix (*gradG1)(const Config& config),
|
||||
// const std::string& key2,
|
||||
// Matrix (*gradG2)(const Config& config),
|
||||
// Vector (*g)(const Config& config),
|
||||
// const Vector& lambdas) :
|
||||
// NonlinearConstraint<Config>(lambdas, g),
|
||||
// gradG1_(gradG1), key1_(key1),
|
||||
// gradG2_(gradG2), key2_(key2) {}
|
||||
//
|
||||
// /** Print */
|
||||
// void print(const std::string& s = "") const {}
|
||||
//
|
||||
// /** Check if two factors are equal */
|
||||
// bool equals(const Factor<Config>& f, double tol=1e-9) const {
|
||||
// return true;
|
||||
// }
|
||||
//
|
||||
// /** Linearize a non-linearFactor2 to get a linearFactor2 */
|
||||
// boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
|
||||
// boost::shared_ptr<GaussianFactor> ret;
|
||||
// return ret;
|
||||
// }
|
||||
//};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
/*
|
||||
* @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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -198,7 +198,7 @@ TEST (SQP, problem1_sqp ) {
|
|||
new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
|
||||
"y", lam*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
|
||||
"lam", eye(1), // dlam term
|
||||
Vector_(1.0, 0.0), // rhs is zero
|
||||
Vector_(1, 0.0), // rhs is zero
|
||||
1.0)); // arbitrary sigma
|
||||
|
||||
// create the actual constraint
|
||||
|
|
@ -318,6 +318,69 @@ TEST (SQP, two_pose_truth ) {
|
|||
CHECK(assert_equal(expected, *actual, 1e-5));
|
||||
}
|
||||
|
||||
// Test of binary nonlinear equality constraint disabled until nonlinear constraints work
|
||||
|
||||
///**
|
||||
// * Version that actually uses nonlinear equality constraints
|
||||
// * to to perform optimization. Same as above, but no
|
||||
// * equality constraint on x2, and two landmarks that
|
||||
// * should be the same.
|
||||
// */
|
||||
//TEST (SQP, two_pose ) {
|
||||
// // position (1, 1) constraint for x1
|
||||
// VectorConfig feas;
|
||||
// feas.insert("x1", Vector_(2, 1.0, 1.0));
|
||||
//
|
||||
// // constant constraint on x1
|
||||
// shared_eq ef1(new NonlinearEquality<VectorConfig>("x1", feas, 2, *vector_compare));
|
||||
//
|
||||
// // measurement from x1 to l1
|
||||
// Vector z1 = Vector_(2, 0.0, 5.0);
|
||||
// double sigma1 = 0.1;
|
||||
// shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
|
||||
//
|
||||
// // measurement from x2 to l2
|
||||
// Vector z2 = Vector_(2, -4.0, 0.0);
|
||||
// double sigma2 = 0.1;
|
||||
// shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
|
||||
//
|
||||
// // equality constraint between l1 and l2
|
||||
// boost::shared_ptr<NonlinearConstraint<VectorConfig> > c1(
|
||||
// new NonlinearConstraint<VectorConfig>(
|
||||
// "l1", "l2", // specify nodes constrained
|
||||
// *g_function, // evaluate the cost
|
||||
// *gradG_function)); // construct the gradient of g(x)
|
||||
//
|
||||
// // construct the graph
|
||||
// NLGraph graph;
|
||||
// graph.push_back(ef1);
|
||||
// graph.push_back(c1);
|
||||
// graph.push_back(f1);
|
||||
// graph.push_back(f2);
|
||||
//
|
||||
// // create an initial estimate
|
||||
// boost::shared_ptr<VectorConfig> initialEstimate(new VectorConfig(feas)); // must start with feasible set
|
||||
// initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth
|
||||
// //initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error
|
||||
//
|
||||
// // optimize the graph
|
||||
// Ordering ordering;
|
||||
// ordering += "x1", "x2", "l1";
|
||||
// Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
|
||||
//
|
||||
// // display solution
|
||||
// double relativeThreshold = 1e-5;
|
||||
// double absoluteThreshold = 1e-5;
|
||||
// Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold);
|
||||
// boost::shared_ptr<const VectorConfig> actual = act_opt.config();
|
||||
// //actual->print("Configuration after optimization");
|
||||
//
|
||||
// // verify
|
||||
// VectorConfig expected(feas);
|
||||
// expected.insert("l1", Vector_(2, 1.0, 6.0));
|
||||
// CHECK(assert_equal(expected, *actual, 1e-5));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue