12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.

release/4.3a0
Alex Cunningham 2009-11-28 19:18:02 +00:00
parent 107c6846fb
commit 0ff7e3a5d9
5 changed files with 81 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
cpp/NonlinearConstraint-inl.h
View File

@ -27,17 +27,18 @@ bool NonlinearConstraint<Config>::active(const Config& config) const {
template <class Config> template <class Config>
NonlinearConstraint1<Config>::NonlinearConstraint1( NonlinearConstraint1<Config>::NonlinearConstraint1(
const std::string& key, const std::string& key,
Matrix (*gradG)(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::string& key), Vector (*g)(const Config& config, const std::list<std::string>& keys),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
g_(g), gradG_(gradG), key_(key) { gradG_(gradG), key_(key) {
// set a good lagrange key here // set a good lagrange key here
// TODO:should do something smart to find a unique one // TODO:should do something smart to find a unique one
if (lagrange_key == "") if (lagrange_key == "")
this->lagrange_key_ = "L_" + 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 (p == NULL) return false;
if (key_ != p->key_) return false; if (key_ != p->key_) return false;
if (this->lagrange_key_ != p->lagrange_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 (gradG_ != p->gradG_) return false;
if (this->isEquality_ != p->isEquality_) return false; if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_; return this->p_ == p->p_;
@ -74,10 +75,10 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
Vector lambda = lagrange[this->lagrange_key_]; Vector lambda = lagrange[this->lagrange_key_];
// find the error // find the error
Vector g = g_(config, key_); Vector g = g_(config, this->keys());
// construct the gradient // construct the gradient
Matrix grad = gradG_(config, key_); Matrix grad = gradG_(config, this->keys());
// construct probabilistic factor // construct probabilistic factor
Matrix A1 = vector_scale(grad, lambda); Matrix A1 = vector_scale(grad, lambda);
@ -98,19 +99,21 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
template <class Config> template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2( NonlinearConstraint2<Config>::NonlinearConstraint2(
const std::string& key1, 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, const std::string& key2,
Matrix (*gradG2)(const Config& config, const std::string& key), Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
Vector (*g)(const Config& config, const std::string& key1, const std::string& key2), Vector (*g)(const Config& config, const std::list<std::string>& keys),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
g_(g), gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) { gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) {
// set a good lagrange key here // set a good lagrange key here
// TODO:should do something smart to find a unique one // TODO:should do something smart to find a unique one
if (lagrange_key == "") if (lagrange_key == "")
this->lagrange_key_ = "L_" + key1 + key2; 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 (key1_ != p->key1_) return false;
if (key2_ != p->key2_) return false; if (key2_ != p->key2_) return false;
if (this->lagrange_key_ != p->lagrange_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 (gradG1_ != p->gradG1_) return false; if (gradG1_ != p->gradG1_) return false;
if (gradG2_ != p->gradG2_) return false; if (gradG2_ != p->gradG2_) return false;
if (this->isEquality_ != p->isEquality_) 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_]; Vector lambda = lagrange[this->lagrange_key_];
// find the error // find the error
Vector g = g_(config, key1_, key2_); Vector g = g_(config, this->keys());
// construct the gradients // construct the gradients
Matrix grad1 = gradG1_(config, key1_); Matrix grad1 = gradG1_(config, this->keys());
Matrix grad2 = gradG2_(config, key2_); Matrix grad2 = gradG2_(config, this->keys());
// construct probabilistic factor // construct probabilistic factor
Matrix A1 = vector_scale(grad1, lambda); Matrix A1 = vector_scale(grad1, lambda);

59
cpp/NonlinearConstraint.h
View File

@ -35,17 +35,29 @@ protected:
/** type of constraint */ /** type of constraint */
bool isEquality_; 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: public:
/** Constructor - sets the cost function and the lagrange multipliers /** Constructor - sets the cost function and the lagrange multipliers
* @param lagrange_key is the label for the associated lagrange multipliers * @param lagrange_key is the label for the associated lagrange multipliers
* @param dim_lagrange is the number of associated constraints * @param dim_lagrange is the number of associated constraints
* @param isEquality is true if the constraint is an equality constraint * @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, NonlinearConstraint(const std::string& lagrange_key,
size_t dim_lagrange,
Vector (*g)(const Config& config, const std::list<std::string>& keys),
bool isEquality=true) bool isEquality=true)
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0), : 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 */ /** returns the key used for the Lagrange multipliers */
std::string lagrangeKey() const { return lagrange_key_; } 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; virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
/** error function - returns the result of the constraint function */ /** 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 * Determines whether the constraint is active given a particular configuration
@ -100,13 +112,6 @@ template <class Config>
class NonlinearConstraint1 : public NonlinearConstraint<Config> { class NonlinearConstraint1 : public NonlinearConstraint<Config> {
private: 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 * Calculates the gradient of the constraint function
@ -115,7 +120,7 @@ private:
* @param key of selected variable * @param key of selected variable
* @return the jacobian of the constraint in terms of key * @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 */ /** key for the constrained variable */
std::string key_; std::string key_;
@ -133,8 +138,8 @@ public:
*/ */
NonlinearConstraint1( NonlinearConstraint1(
const std::string& key, const std::string& key,
Matrix (*gradG)(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::string& key), Vector (*g)(const Config& config, const std::list<std::string>& keys),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -145,11 +150,6 @@ public:
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol=1e-9) const; 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 * Linearize using a real Config and a VectorConfig of Lagrange multipliers
* Returns the two separate Gaussian factors to solve * Returns the two separate Gaussian factors to solve
@ -168,14 +168,6 @@ template <class Config>
class NonlinearConstraint2 : public NonlinearConstraint<Config> { class NonlinearConstraint2 : public NonlinearConstraint<Config> {
private: 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 * Calculates the gradients of the constraint function in terms of
@ -185,8 +177,8 @@ private:
* @param key of selected variable * @param key of selected variable
* @return the jacobian of the constraint in terms of key * @return the jacobian of the constraint in terms of key
*/ */
Matrix (*gradG1_) (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::string& key); Matrix (*gradG2_) (const Config& config, const std::list<std::string>& keys);
/** keys for the constrained variables */ /** keys for the constrained variables */
std::string key1_; std::string key1_;
@ -205,10 +197,10 @@ public:
*/ */
NonlinearConstraint2( NonlinearConstraint2(
const std::string& key1, 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, const std::string& key2,
Matrix (*gradG2)(const Config& config, const std::string& key), Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
Vector (*g)(const Config& config, const std::string& key1, const std::string& key2), Vector (*g)(const Config& config, const std::list<std::string>& keys),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -219,11 +211,6 @@ public:
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol=1e-9) const; 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 * Linearize using a real Config and a VectorConfig of Lagrange multipliers
* Returns the two separate Gaussian factors to solve * Returns the two separate Gaussian factors to solve

31
cpp/testNonlinearConstraint.cpp
View File

@ -9,6 +9,7 @@
#include <NonlinearConstraint.h> #include <NonlinearConstraint.h>
#include <NonlinearConstraint-inl.h> #include <NonlinearConstraint-inl.h>
using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
@ -16,14 +17,14 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
namespace test1 { namespace test1 {
/** p = 1, gradG(x) = 2x */ /** p = 1, gradG(x) = 2x */
Matrix grad_g(const VectorConfig& config, const std::string& key) { Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
double x = config[key](0); double x = config[keys.front()](0);
return Matrix_(1,1, 2*x); return Matrix_(1,1, 2*x);
} }
/** p = 1, g(x) = x^2-5 = 0 */ /** p = 1, g(x) = x^2-5 = 0 */
Vector g_func(const VectorConfig& config, const std::string& key) { Vector g_func(const VectorConfig& config, const list<string>& keys) {
double x = config[key](0); double x = config[keys.front()](0);
return Vector_(1, x*x-5); return Vector_(1, x*x-5);
} }
} // \namespace test1 } // \namespace test1
@ -89,21 +90,21 @@ TEST( NonlinearConstraint1, unary_scalar_equal ) {
/* ************************************************************************* */ /* ************************************************************************* */
namespace test2 { namespace test2 {
/** gradient for x, gradG(x,y) in x: 2x*/ /** gradient for x, gradG(x,y) in x: 2x*/
Matrix grad_g1(const VectorConfig& config, const std::string& key) { Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
double x = config[key](0); double x = config[keys.front()](0);
return Matrix_(1,1, 2.0*x); return Matrix_(1,1, 2.0*x);
} }
/** gradient for y, gradG(x,y) in y: -1 */ /** gradient for y, gradG(x,y) in y: -1 */
Matrix grad_g2(const VectorConfig& config, const std::string& key) { Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
double x = config[key](0); double x = config[keys.back()](0);
return Matrix_(1,1, -1.0); return Matrix_(1,1, -1.0);
} }
/** p = 1, g(x) = x^2-5 -y = 0 */ /** p = 1, g(x) = x^2-5 -y = 0 */
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { Vector g_func(const VectorConfig& config, const list<string>& keys) {
double x = config[key1](0); double x = config[keys.front()](0);
double y = config[key2](0); double y = config[keys.back()](0);
return Vector_(1, x*x-5.0-y); return Vector_(1, x*x-5.0-y);
} }
} // \namespace test2 } // \namespace test2
@ -182,14 +183,14 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
/* ************************************************************************* */ /* ************************************************************************* */
namespace inequality_unary { namespace inequality_unary {
/** p = 1, gradG(x) = 2*x */ /** p = 1, gradG(x) = 2*x */
Matrix grad_g(const VectorConfig& config, const std::string& key) { Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
double x = config[key](0); double x = config[keys.front()](0);
return Matrix_(1,1, 2*x); return Matrix_(1,1, 2*x);
} }
/** p = 1, g(x) x^2 - 5 > 0 */ /** p = 1, g(x) x^2 - 5 > 0 */
Vector g_func(const VectorConfig& config, const std::string& key) { Vector g_func(const VectorConfig& config, const list<string>& keys) {
double x = config[key](0); double x = config[keys.front()](0);
double g = x*x-5; double g = x*x-5;
if (g > 0) if (g > 0)
return Vector_(1, 0.0); // return exactly zero return Vector_(1, 0.0); // return exactly zero

22
cpp/testSQP.cpp
View File

@ -320,17 +320,17 @@ TEST (SQP, two_pose_truth ) {
namespace sqp_test1 { namespace sqp_test1 {
// binary constraint between landmarks // binary constraint between landmarks
/** g(x) = x-y = 0 */ /** g(x) = x-y = 0 */
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { Vector g_func(const VectorConfig& config, const list<string>& keys) {
return config[key1]-config[key2]; return config[keys.front()]-config[keys.back()];
} }
/** gradient at l1 */ /** 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); return eye(2);
} }
/** gradient at l2 */ /** 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); return -1*eye(2);
} }
} // \namespace sqp_test1 } // \namespace sqp_test1
@ -338,12 +338,12 @@ Matrix grad_g2(const VectorConfig& config, const std::string& key) {
namespace sqp_test2 { namespace sqp_test2 {
// Unary Constraint on x1 // Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */ /** g(x) = x -[1;1] = 0 */
Vector g_func(const VectorConfig& config, const std::string& key) { Vector g_func(const VectorConfig& config, const list<string>& keys) {
return config[key]-Vector_(2, 1.0, 1.0); return config[keys.front()]-Vector_(2, 1.0, 1.0);
} }
/** gradient at x1 */ /** 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); return eye(2);
} }
} // \namespace sqp_test2 } // \namespace sqp_test2
@ -635,17 +635,17 @@ int getNum(const string& key) {
namespace sqp_stereo { namespace sqp_stereo {
// binary constraint between landmarks // binary constraint between landmarks
/** g(x) = x-y = 0 */ /** g(x) = x-y = 0 */
Vector g_func(const VSLAMConfig& config, const std::string& key1, const std::string& key2) { Vector g_func(const VSLAMConfig& config, const list<string>& keys) {
return config.landmarkPoint(getNum(key1)).vector()-config.landmarkPoint(getNum(key2)).vector(); return config.landmarkPoint(getNum(keys.front())).vector()-config.landmarkPoint(getNum(keys.back())).vector();
} }
/** gradient at l1 */ /** 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); return eye(3);
} }
/** gradient at l2 */ /** 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); return -1.0*eye(3);
} }
} // \namespace sqp_test1 } // \namespace sqp_test1

22
cpp/testSQPOptimizer.cpp
View File

@ -53,17 +53,17 @@ TEST ( SQPOptimizer, basic ) {
namespace sqp_LinearMapWarp2 { namespace sqp_LinearMapWarp2 {
// binary constraint between landmarks // binary constraint between landmarks
/** g(x) = x-y = 0 */ /** g(x) = x-y = 0 */
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { Vector g_func(const VectorConfig& config, const list<string>& keys) {
return config[key1]-config[key2]; return config[keys.front()]-config[keys.back()];
} }
/** gradient at l1 */ /** 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); return eye(2);
} }
/** gradient at l2 */ /** 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); return -1*eye(2);
} }
} // \namespace sqp_LinearMapWarp2 } // \namespace sqp_LinearMapWarp2
@ -71,12 +71,12 @@ Matrix grad_g2(const VectorConfig& config, const std::string& key) {
namespace sqp_LinearMapWarp1 { namespace sqp_LinearMapWarp1 {
// Unary Constraint on x1 // Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */ /** g(x) = x -[1;1] = 0 */
Vector g_func(const VectorConfig& config, const std::string& key) { Vector g_func(const VectorConfig& config, const list<string>& keys) {
return config[key]-Vector_(2, 1.0, 1.0); return config[keys.front()]-Vector_(2, 1.0, 1.0);
} }
/** gradient at x1 */ /** 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); return eye(2);
} }
} // \namespace sqp_LinearMapWarp12 } // \namespace sqp_LinearMapWarp12
@ -223,8 +223,8 @@ double radius = 1.0;
// binary avoidance constraint // binary avoidance constraint
/** g(x) = ||x2-obs||^2 > radius^2 */ /** g(x) = ||x2-obs||^2 > radius^2 */
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { Vector g_func(const VectorConfig& config, const list<string>& keys) {
Vector delta = config[key1]-config[key2]; Vector delta = config[keys.front()]-config[keys.back()];
double dist2 = sum(emul(delta, delta)); double dist2 = sum(emul(delta, delta));
double thresh = radius*radius; double thresh = radius*radius;
if (dist2 <= thresh) if (dist2 <= thresh)
@ -234,13 +234,13 @@ Vector g_func(const VectorConfig& config, const std::string& key1, const std::st
} }
/** gradient at pose */ /** 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; Matrix grad;
return grad; return grad;
} }
/** gradient at obstacle */ /** 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); return -1*eye(1);
} }
} }