Changed interface on constraint g(x) and grad_g(x) functions to take a list of keys, so that all of the variables in a factor can be used as necessary. Moved g(x) into base NonlinearConstraint class and some cleanup.
Alex Cunningham
parent
107c6846fb
commit
0ff7e3a5d9
|
|
@ -27,17 +27,18 @@ bool NonlinearConstraint<Config>::active(const Config& config) const {
|
|||
template <class Config>
|
||||
NonlinearConstraint1<Config>::NonlinearConstraint1(
|
||||
const std::string& key,
|
||||
Matrix (*gradG)(const Config& config, const std::string& key),
|
||||
Vector (*g)(const Config& config, const std::string& key),
|
||||
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
|
||||
g_(g), gradG_(gradG), key_(key) {
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -60,7 +61,7 @@ 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 (g_ != p->g_) 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_;
|
||||
|
|
@ -74,10 +75,10 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
|
|||
Vector lambda = lagrange[this->lagrange_key_];
|
||||
|
||||
// find the error
|
||||
Vector g = g_(config, key_);
|
||||
Vector g = g_(config, this->keys());
|
||||
|
||||
// construct the gradient
|
||||
Matrix grad = gradG_(config, key_);
|
||||
Matrix grad = gradG_(config, this->keys());
|
||||
|
||||
// construct probabilistic factor
|
||||
Matrix A1 = vector_scale(grad, lambda);
|
||||
|
|
@ -98,19 +99,21 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
|
|||
template <class Config>
|
||||
NonlinearConstraint2<Config>::NonlinearConstraint2(
|
||||
const std::string& key1,
|
||||
Matrix (*gradG1)(const Config& config, const std::string& key),
|
||||
Matrix (*gradG1)(const Config& config, const std::list<std::string>& keys),
|
||||
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),
|
||||
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
|
||||
g_(g), gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) {
|
||||
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 == "")
|
||||
this->lagrange_key_ = "L_" + key1 + key2;
|
||||
this->keys_.push_front(key1);
|
||||
this->keys_.push_back(key2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -131,7 +134,7 @@ 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 (g_ != p->g_) 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;
|
||||
|
|
@ -146,11 +149,11 @@ NonlinearConstraint2<Config>::linearize(const Config& config, const VectorConfig
|
|||
Vector lambda = lagrange[this->lagrange_key_];
|
||||
|
||||
// find the error
|
||||
Vector g = g_(config, key1_, key2_);
|
||||
Vector g = g_(config, this->keys());
|
||||
|
||||
// construct the gradients
|
||||
Matrix grad1 = gradG1_(config, key1_);
|
||||
Matrix grad2 = gradG2_(config, key2_);
|
||||
Matrix grad1 = gradG1_(config, this->keys());
|
||||
Matrix grad2 = gradG2_(config, this->keys());
|
||||
|
||||
// construct probabilistic factor
|
||||
Matrix A1 = vector_scale(grad1, lambda);
|
||||
|
|
|
|||
|
|
@ -35,17 +35,29 @@ protected:
|
|||
/** type of constraint */
|
||||
bool isEquality_;
|
||||
|
||||
/** 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 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);
|
||||
|
||||
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
|
||||
* @param isEquality is true if the constraint is an equality constraint
|
||||
* @param g is the cost function for the constraint
|
||||
*/
|
||||
NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange,
|
||||
bool isEquality=true)
|
||||
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) {}
|
||||
lagrange_key_(lagrange_key), p_(dim_lagrange),
|
||||
isEquality_(isEquality), g_(g) {}
|
||||
|
||||
/** returns the key used for the Lagrange multipliers */
|
||||
std::string lagrangeKey() const { return lagrange_key_; }
|
||||
|
|
@ -63,7 +75,7 @@ public:
|
|||
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 { return zero(1); }
|
||||
inline Vector error_vector(const Config& c) const { return g_(c, this->keys()); }
|
||||
|
||||
/**
|
||||
* Determines whether the constraint is active given a particular configuration
|
||||
|
|
@ -100,13 +112,6 @@ template <class Config>
|
|||
class NonlinearConstraint1 : 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 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
|
||||
|
|
@ -115,7 +120,7 @@ private:
|
|||
* @param key of selected variable
|
||||
* @return the jacobian of the constraint in terms of key
|
||||
*/
|
||||
Matrix (*gradG_) (const Config& config, const std::string& key);
|
||||
Matrix (*gradG_) (const Config& config, const std::list<std::string>& keys);
|
||||
|
||||
/** key for the constrained variable */
|
||||
std::string key_;
|
||||
|
|
@ -133,8 +138,8 @@ public:
|
|||
*/
|
||||
NonlinearConstraint1(
|
||||
const std::string& key,
|
||||
Matrix (*gradG)(const Config& config, const std::string& key),
|
||||
Vector (*g)(const Config& config, const std::string& key),
|
||||
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
@ -145,11 +150,6 @@ public:
|
|||
/** 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, key_);
|
||||
}
|
||||
|
||||
/**
|
||||
* Linearize using a real Config and a VectorConfig of Lagrange multipliers
|
||||
* Returns the two separate Gaussian factors to solve
|
||||
|
|
@ -168,14 +168,6 @@ 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);
|
||||
|
||||
/**
|
||||
* Calculates the gradients of the constraint function in terms of
|
||||
|
|
@ -185,8 +177,8 @@ private:
|
|||
* @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);
|
||||
Matrix (*gradG1_) (const Config& config, const std::list<std::string>& keys);
|
||||
Matrix (*gradG2_) (const Config& config, const std::list<std::string>& keys);
|
||||
|
||||
/** keys for the constrained variables */
|
||||
std::string key1_;
|
||||
|
|
@ -205,10 +197,10 @@ public:
|
|||
*/
|
||||
NonlinearConstraint2(
|
||||
const std::string& key1,
|
||||
Matrix (*gradG1)(const Config& config, const std::string& key),
|
||||
Matrix (*gradG1)(const Config& config, const std::list<std::string>& keys),
|
||||
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),
|
||||
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
@ -219,11 +211,6 @@ public:
|
|||
/** 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
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
#include <NonlinearConstraint.h>
|
||||
#include <NonlinearConstraint-inl.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -16,14 +17,14 @@ using namespace gtsam;
|
|||
/* ************************************************************************* */
|
||||
namespace test1 {
|
||||
/** p = 1, gradG(x) = 2x */
|
||||
Matrix grad_g(const VectorConfig& config, const std::string& key) {
|
||||
double x = config[key](0);
|
||||
Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](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);
|
||||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
return Vector_(1, x*x-5);
|
||||
}
|
||||
} // \namespace test1
|
||||
|
|
@ -89,21 +90,21 @@ TEST( NonlinearConstraint1, unary_scalar_equal ) {
|
|||
/* ************************************************************************* */
|
||||
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);
|
||||
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](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);
|
||||
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.back()](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);
|
||||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
double y = config[keys.back()](0);
|
||||
return Vector_(1, x*x-5.0-y);
|
||||
}
|
||||
} // \namespace test2
|
||||
|
|
@ -182,14 +183,14 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
|
|||
/* ************************************************************************* */
|
||||
namespace inequality_unary {
|
||||
/** p = 1, gradG(x) = 2*x */
|
||||
Matrix grad_g(const VectorConfig& config, const std::string& key) {
|
||||
double x = config[key](0);
|
||||
Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](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);
|
||||
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
|
||||
|
|
|
|||
|
|
@ -320,17 +320,17 @@ TEST (SQP, two_pose_truth ) {
|
|||
namespace sqp_test1 {
|
||||
// binary constraint between landmarks
|
||||
/** g(x) = x-y = 0 */
|
||||
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) {
|
||||
return config[key1]-config[key2];
|
||||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
return config[keys.front()]-config[keys.back()];
|
||||
}
|
||||
|
||||
/** gradient at l1 */
|
||||
Matrix grad_g1(const VectorConfig& config, const std::string& key) {
|
||||
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
|
||||
return eye(2);
|
||||
}
|
||||
|
||||
/** gradient at l2 */
|
||||
Matrix grad_g2(const VectorConfig& config, const std::string& key) {
|
||||
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
|
||||
return -1*eye(2);
|
||||
}
|
||||
} // \namespace sqp_test1
|
||||
|
|
@ -338,12 +338,12 @@ Matrix grad_g2(const VectorConfig& config, const std::string& key) {
|
|||
namespace sqp_test2 {
|
||||
// Unary Constraint on x1
|
||||
/** g(x) = x -[1;1] = 0 */
|
||||
Vector g_func(const VectorConfig& config, const std::string& key) {
|
||||
return config[key]-Vector_(2, 1.0, 1.0);
|
||||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
return config[keys.front()]-Vector_(2, 1.0, 1.0);
|
||||
}
|
||||
|
||||
/** gradient at x1 */
|
||||
Matrix grad_g(const VectorConfig& config, const std::string& key) {
|
||||
Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
|
||||
return eye(2);
|
||||
}
|
||||
} // \namespace sqp_test2
|
||||
|
|
@ -635,17 +635,17 @@ int getNum(const string& key) {
|
|||
namespace sqp_stereo {
|
||||
// binary constraint between landmarks
|
||||
/** g(x) = x-y = 0 */
|
||||
Vector g_func(const VSLAMConfig& config, const std::string& key1, const std::string& key2) {
|
||||
return config.landmarkPoint(getNum(key1)).vector()-config.landmarkPoint(getNum(key2)).vector();
|
||||
Vector g_func(const VSLAMConfig& config, const list<string>& keys) {
|
||||
return config.landmarkPoint(getNum(keys.front())).vector()-config.landmarkPoint(getNum(keys.back())).vector();
|
||||
}
|
||||
|
||||
/** gradient at l1 */
|
||||
Matrix grad_g1(const VSLAMConfig& config, const std::string& key) {
|
||||
Matrix grad_g1(const VSLAMConfig& config, const list<string>& keys) {
|
||||
return eye(3);
|
||||
}
|
||||
|
||||
/** gradient at l2 */
|
||||
Matrix grad_g2(const VSLAMConfig& config, const std::string& key) {
|
||||
Matrix grad_g2(const VSLAMConfig& config, const list<string>& keys) {
|
||||
return -1.0*eye(3);
|
||||
}
|
||||
} // \namespace sqp_test1
|
||||
|
|
|
|||
|
|
@ -53,17 +53,17 @@ TEST ( SQPOptimizer, basic ) {
|
|||
namespace sqp_LinearMapWarp2 {
|
||||
// binary constraint between landmarks
|
||||
/** g(x) = x-y = 0 */
|
||||
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) {
|
||||
return config[key1]-config[key2];
|
||||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
return config[keys.front()]-config[keys.back()];
|
||||
}
|
||||
|
||||
/** gradient at l1 */
|
||||
Matrix grad_g1(const VectorConfig& config, const std::string& key) {
|
||||
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
|
||||
return eye(2);
|
||||
}
|
||||
|
||||
/** gradient at l2 */
|
||||
Matrix grad_g2(const VectorConfig& config, const std::string& key) {
|
||||
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
|
||||
return -1*eye(2);
|
||||
}
|
||||
} // \namespace sqp_LinearMapWarp2
|
||||
|
|
@ -71,12 +71,12 @@ Matrix grad_g2(const VectorConfig& config, const std::string& key) {
|
|||
namespace sqp_LinearMapWarp1 {
|
||||
// Unary Constraint on x1
|
||||
/** g(x) = x -[1;1] = 0 */
|
||||
Vector g_func(const VectorConfig& config, const std::string& key) {
|
||||
return config[key]-Vector_(2, 1.0, 1.0);
|
||||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
return config[keys.front()]-Vector_(2, 1.0, 1.0);
|
||||
}
|
||||
|
||||
/** gradient at x1 */
|
||||
Matrix grad_g(const VectorConfig& config, const std::string& key) {
|
||||
Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
|
||||
return eye(2);
|
||||
}
|
||||
} // \namespace sqp_LinearMapWarp12
|
||||
|
|
@ -223,8 +223,8 @@ double radius = 1.0;
|
|||
|
||||
// binary avoidance constraint
|
||||
/** g(x) = ||x2-obs||^2 > radius^2 */
|
||||
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) {
|
||||
Vector delta = config[key1]-config[key2];
|
||||
Vector g_func(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;
|
||||
if (dist2 <= thresh)
|
||||
|
|
@ -234,13 +234,13 @@ Vector g_func(const VectorConfig& config, const std::string& key1, const std::st
|
|||
}
|
||||
|
||||
/** gradient at pose */
|
||||
Matrix grad_g1(const VectorConfig& config, const std::string& key) {
|
||||
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
|
||||
Matrix grad;
|
||||
return grad;
|
||||
}
|
||||
|
||||
/** gradient at obstacle */
|
||||
Matrix grad_g2(const VectorConfig& config, const std::string& key) {
|
||||
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
|
||||
return -1*eye(1);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue