Basic implementation of a binary nonlinear constraint, with working linearization of a binary constraint.
Alex Cunningham
parent
ee4a066275
commit
714edb57f0
|
|
@ -11,18 +11,20 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Implementations of unary nonlinear constraints
|
||||
*/
|
||||
/* ************************************************************************* */
|
||||
// Implementations of unary nonlinear constraints
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
void NonlinearConstraint1<Config>::print(const std::string& s) const {
|
||||
std::cout << "NonlinearConstraint [" << s << "]:\n"
|
||||
<< " Variable: " << key_ << "\n"
|
||||
std::cout << "NonlinearConstraint1 [" << s << "]:\n"
|
||||
<< " key: " << key_ << "\n"
|
||||
<< " p: " << this->p_ << "\n"
|
||||
<< " lambda key: " << this->lagrange_key_ << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
bool NonlinearConstraint1<Config>::equals(const Factor<Config>& f, double tol) const {
|
||||
const NonlinearConstraint1<Config>* p = dynamic_cast<const NonlinearConstraint1<Config>*> (&f);
|
||||
|
|
@ -34,6 +36,7 @@ bool NonlinearConstraint1<Config>::equals(const Factor<Config>& f, double tol) c
|
|||
return this->p_ == p->p_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
|
||||
NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
|
||||
|
|
@ -52,7 +55,62 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
|
|||
GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
|
||||
|
||||
// construct the constraint
|
||||
GaussianFactor::shared_ptr constraint(new GaussianFactor("x", grad, g, 0.0));
|
||||
GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, g, 0.0));
|
||||
|
||||
return std::make_pair(factor, constraint);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Implementations of binary nonlinear constraints
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
void NonlinearConstraint2<Config>::print(const std::string& s) const {
|
||||
std::cout << "NonlinearConstraint2 [" << s << "]:\n"
|
||||
<< " key1: " << key1_ << "\n"
|
||||
<< " key2: " << key2_ << "\n"
|
||||
<< " p: " << this->p_ << "\n"
|
||||
<< " lambda key: " << this->lagrange_key_ << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
bool NonlinearConstraint2<Config>::equals(const Factor<Config>& f, double tol) const {
|
||||
const NonlinearConstraint2<Config>* p = dynamic_cast<const NonlinearConstraint2<Config>*> (&f);
|
||||
if (p == NULL) return false;
|
||||
if (key1_ != p->key1_) return false;
|
||||
if (key2_ != p->key2_) return false;
|
||||
if (this->lagrange_key_ != p->lagrange_key_) return false;
|
||||
if (g_ != p->g_) return false;
|
||||
if (gradG1_ != p->gradG1_) return false;
|
||||
if (gradG2_ != p->gradG2_) return false;
|
||||
return this->p_ == p->p_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
|
||||
NonlinearConstraint2<Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
|
||||
// extract lagrange multiplier
|
||||
Vector lambda = lagrange[this->lagrange_key_];
|
||||
|
||||
// find the error
|
||||
Vector g = g_(config, key1_, key2_);
|
||||
|
||||
// construct the gradients
|
||||
Matrix grad1 = gradG1_(config, key1_);
|
||||
Matrix grad2 = gradG2_(config, key2_);
|
||||
|
||||
// construct probabilistic factor
|
||||
Matrix A1 = vector_scale(grad1, lambda);
|
||||
Matrix A2 = vector_scale(grad2, lambda);
|
||||
GaussianFactor::shared_ptr factor(new
|
||||
GaussianFactor(key1_, A1, key2_, A2,
|
||||
this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
|
||||
|
||||
// construct the constraint
|
||||
GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, g, 0.0));
|
||||
|
||||
return std::make_pair(factor, constraint);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -151,65 +151,83 @@ public:
|
|||
linearize(const Config& config, const VectorConfig& lagrange) const;
|
||||
};
|
||||
|
||||
/**
|
||||
* A binary constraint with arbitrary cost and gradient functions
|
||||
*/
|
||||
template <class Config>
|
||||
class NonlinearConstraint2 : public 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 key1 is the id for the first variable
|
||||
* @param key2 is the id for the second variable
|
||||
* @return the cost for each of p constraints, arranged in a vector
|
||||
*/
|
||||
Vector (*g_)(const Config& config, const std::string& key1, const std::string& key2);
|
||||
|
||||
//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;
|
||||
// }
|
||||
//};
|
||||
/**
|
||||
* Calculates the gradients of the constraint function in terms of
|
||||
* the first and second variables
|
||||
* 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 (*gradG1_) (const Config& config, const std::string& key);
|
||||
Matrix (*gradG2_) (const Config& config, const std::string& key);
|
||||
|
||||
/** keys for the constrained variables */
|
||||
std::string key1_;
|
||||
std::string key2_;
|
||||
|
||||
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
|
||||
*/
|
||||
NonlinearConstraint2(
|
||||
const std::string& key1,
|
||||
Matrix (*gradG1)(const Config& config, const std::string& key),
|
||||
const std::string& key2,
|
||||
Matrix (*gradG2)(const Config& config, const std::string& key),
|
||||
Vector (*g)(const Config& config, const std::string& key1, const std::string& key2),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="") :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint),
|
||||
g_(g), 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 == "")
|
||||
this->lagrange_key_ = "L_" + key1 + 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;
|
||||
|
||||
/** error function - returns the result of the constraint function */
|
||||
inline Vector error_vector(const Config& c) const {
|
||||
return g_(c, key1_, key2_);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ Vector g_func(const VectorConfig& config, const std::string& key) {
|
|||
} // \namespace test1
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint, unary_scalar_construction ) {
|
||||
TEST( NonlinearConstraint1, unary_scalar_construction ) {
|
||||
// construct a constraint on x
|
||||
// the lagrange multipliers will be expected on L_x1
|
||||
// and there is only one multiplier
|
||||
|
|
@ -47,10 +47,7 @@ TEST( NonlinearConstraint, unary_scalar_construction ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint, unary_scalar_linearize ) {
|
||||
// construct a constraint on x
|
||||
// the lagrange multipliers will be expected on L_x1
|
||||
// and there is only one multiplier
|
||||
TEST( NonlinearConstraint1, unary_scalar_linearize ) {
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *test1::grad_g, *test1::g_func, p, "L_x1");
|
||||
|
||||
|
|
@ -74,7 +71,7 @@ TEST( NonlinearConstraint, unary_scalar_linearize ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint, unary_scalar_equal ) {
|
||||
TEST( NonlinearConstraint1, unary_scalar_equal ) {
|
||||
NonlinearConstraint1<VectorConfig>
|
||||
c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1"),
|
||||
c2("x", *test1::grad_g, *test1::g_func, 1, "L_x1"),
|
||||
|
|
@ -87,6 +84,99 @@ TEST( NonlinearConstraint, unary_scalar_equal ) {
|
|||
CHECK(!c1.equals(c4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// binary functions with scalar variables
|
||||
/* ************************************************************************* */
|
||||
namespace test2 {
|
||||
/** gradient for x, gradG(x,y) in x: 2x*/
|
||||
Matrix grad_g1(const VectorConfig& config, const std::string& key) {
|
||||
double x = config[key](0);
|
||||
return Matrix_(1,1, 2.0*x);
|
||||
}
|
||||
|
||||
/** gradient for y, gradG(x,y) in y: -1 */
|
||||
Matrix grad_g2(const VectorConfig& config, const std::string& key) {
|
||||
double x = config[key](0);
|
||||
return Matrix_(1,1, -1.0);
|
||||
}
|
||||
|
||||
/** p = 1, g(x) = x^2-5 -y = 0 */
|
||||
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) {
|
||||
double x = config[key1](0);
|
||||
double y = config[key2](0);
|
||||
return Vector_(1, x*x-5.0-y);
|
||||
}
|
||||
} // \namespace test2
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_construction ) {
|
||||
// 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;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
"x", *test2::grad_g1,
|
||||
"y", *test2::grad_g2,
|
||||
*test2::g_func, 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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_linearize ) {
|
||||
// create a constraint
|
||||
size_t p = 1;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
"x", *test2::grad_g1,
|
||||
"y", *test2::grad_g2,
|
||||
*test2::g_func, p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig realconfig;
|
||||
realconfig.insert("x", Vector_(1, 1.0));
|
||||
realconfig.insert("y", Vector_(1, 2.0));
|
||||
|
||||
// get a configuration of Lagrange multipliers
|
||||
VectorConfig lagrangeConfig;
|
||||
lagrangeConfig.insert("L_xy", Vector_(1, 3.0));
|
||||
|
||||
// linearize the system
|
||||
GaussianFactor::shared_ptr actFactor, actConstraint;
|
||||
boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig);
|
||||
|
||||
// verify
|
||||
GaussianFactor expFactor("x", Matrix_(1,1, 6.0),
|
||||
"y", Matrix_(1,1, -3.0),
|
||||
"L_xy", eye(1), zero(1), 1.0);
|
||||
GaussianFactor expConstraint("x", Matrix_(1,1, 2.0),
|
||||
"y", Matrix_(1,1, -1.0),
|
||||
Vector_(1,-6.0), 0.0);
|
||||
CHECK(assert_equal(*actFactor, expFactor));
|
||||
CHECK(assert_equal(*actConstraint, expConstraint));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_equal ) {
|
||||
NonlinearConstraint2<VectorConfig>
|
||||
c1("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
|
||||
c2("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
|
||||
c3("y", *test2::grad_g1, "x", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
|
||||
c4("x", *test2::grad_g1, "z", *test2::grad_g2,*test2::g_func, 3, "L_xy");
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
CHECK(!c1.equals(c3));
|
||||
CHECK(!c1.equals(c4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue