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

Renaming gradients -> jacobians

release/4.3a0
Alex Cunningham 2009-12-18 03:05:47 +00:00
parent a3deb992c4
commit e2bc13a2a6
5 changed files with 61 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/NonlinearConstraint-inl.h
View File

@ -55,7 +55,7 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
gradG_(boost::bind(gradG, _1)), key_(key) G_(boost::bind(gradG, _1)), 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
@ -74,7 +74,7 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
gradG_(gradG), key_(key) G_(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
@ -118,7 +118,7 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
Vector g = g_(config); Vector g = g_(config);
// construct the gradient // construct the gradient
Matrix grad = gradG_(config); Matrix grad = G_(config);
// construct probabilistic factor // construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad); Matrix A1 = vector_scale(lambda, grad);
@ -140,14 +140,14 @@ template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2( NonlinearConstraint2<Config>::NonlinearConstraint2(
Vector (*g)(const Config& config), Vector (*g)(const Config& config),
const std::string& key1, const std::string& key1,
Matrix (*gradG1)(const Config& config), Matrix (*G1)(const Config& config),
const std::string& key2, const std::string& key2,
Matrix (*gradG2)(const Config& config), Matrix (*G2)(const Config& config),
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, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
gradG1_(boost::bind(gradG1, _1)), gradG2_(boost::bind(gradG2, _1)), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
// set a good lagrange key here // set a good lagrange key here
@ -163,14 +163,14 @@ template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2( NonlinearConstraint2<Config>::NonlinearConstraint2(
boost::function<Vector(const Config& config)> g, boost::function<Vector(const Config& config)> g,
const std::string& key1, const std::string& key1,
boost::function<Matrix(const Config& config)> gradG1, boost::function<Matrix(const Config& config)> G1,
const std::string& key2, const std::string& key2,
boost::function<Matrix(const Config& config)> gradG2, boost::function<Matrix(const Config& config)> G2,
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, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
gradG1_(gradG1), gradG2_(gradG2), G1_(G1), G2_(G2),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
// set a good lagrange key here // set a good lagrange key here
@ -214,8 +214,8 @@ std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConst
Vector g = g_(config); Vector g = g_(config);
// construct the gradients // construct the gradients
Matrix grad1 = gradG1_(config); Matrix grad1 = G1_(config);
Matrix grad2 = gradG2_(config); Matrix grad2 = G2_(config);
// construct probabilistic factor // construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad1); Matrix A1 = vector_scale(lambda, grad1);

34
cpp/NonlinearConstraint.h
View File

@ -115,7 +115,7 @@ public:
/** /**
* A unary constraint with arbitrary cost and gradient functions * A unary constraint with arbitrary cost and jacobian functions
*/ */
template <class Config> template <class Config>
class NonlinearConstraint1 : public NonlinearConstraint<Config> { class NonlinearConstraint1 : public NonlinearConstraint<Config> {
@ -123,13 +123,13 @@ class NonlinearConstraint1 : public NonlinearConstraint<Config> {
private: private:
/** /**
* Calculates the gradient of the constraint function * Calculates the jacobian of the constraint function
* returns a pxn matrix * returns a pxn matrix
* Use boost.bind to create the function object * Use boost.bind to create the function object
* @param config to use for linearization * @param config to use for linearization
* @return the jacobian of the constraint in terms of key * @return the jacobian of the constraint in terms of key
*/ */
boost::function<Matrix(const Config& config)> gradG_; boost::function<Matrix(const Config& config)> G_;
/** key for the constrained variable */ /** key for the constrained variable */
std::string key_; std::string key_;
@ -139,7 +139,7 @@ public:
/** /**
* Basic constructor * Basic constructor
* @param key is the identifier for the variable constrained * @param key is the identifier for the variable constrained
* @param gradG gives the gradient of the constraint function * @param G gives the jacobian of the constraint function
* @param g is the constraint function * @param g is the constraint function
* @param dim_constraint is the size of the constraint (p) * @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier * @param lagrange_key is the identifier for the lagrange multiplier
@ -148,7 +148,7 @@ public:
NonlinearConstraint1( NonlinearConstraint1(
Vector (*g)(const Config& config), Vector (*g)(const Config& config),
const std::string& key, const std::string& key,
Matrix (*gradG)(const Config& config), Matrix (*G)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -156,7 +156,7 @@ public:
/** /**
* Basic constructor with boost function pointers * Basic constructor with boost function pointers
* @param key is the identifier for the variable constrained * @param key is the identifier for the variable constrained
* @param gradG gives the gradient of the constraint function * @param G gives the jacobian of the constraint function
* @param g is the constraint function as a boost function pointer * @param g is the constraint function as a boost function pointer
* @param dim_constraint is the size of the constraint (p) * @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier * @param lagrange_key is the identifier for the lagrange multiplier
@ -165,7 +165,7 @@ public:
NonlinearConstraint1( NonlinearConstraint1(
boost::function<Vector(const Config& config)> g, boost::function<Vector(const Config& config)> g,
const std::string& key, const std::string& key,
boost::function<Matrix(const Config& config)> gradG, boost::function<Matrix(const Config& config)> G,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -188,7 +188,7 @@ public:
}; };
/** /**
* A binary constraint with arbitrary cost and gradient functions * A binary constraint with arbitrary cost and jacobian functions
*/ */
template <class Config> template <class Config>
class NonlinearConstraint2 : public NonlinearConstraint<Config> { class NonlinearConstraint2 : public NonlinearConstraint<Config> {
@ -196,14 +196,14 @@ class NonlinearConstraint2 : public NonlinearConstraint<Config> {
private: private:
/** /**
* Calculates the gradients of the constraint function in terms of * Calculates the jacobians of the constraint function in terms of
* the first and second variables * the first and second variables
* returns a pxn matrix * returns a pxn matrix
* @param config to use for linearization * @param config to use for linearization
* @return the jacobian of the constraint in terms of key * @return the jacobian of the constraint in terms of key
*/ */
boost::function<Matrix(const Config& config)> gradG1_; boost::function<Matrix(const Config& config)> G1_;
boost::function<Matrix(const Config& config)> gradG2_; boost::function<Matrix(const Config& config)> G2_;
/** keys for the constrained variables */ /** keys for the constrained variables */
std::string key1_; std::string key1_;
@ -214,7 +214,7 @@ public:
/** /**
* Basic constructor * Basic constructor
* @param key is the identifier for the variable constrained * @param key is the identifier for the variable constrained
* @param gradG gives the gradient of the constraint function * @param G gives the jacobian of the constraint function
* @param g is the constraint function * @param g is the constraint function
* @param dim_constraint is the size of the constraint (p) * @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier * @param lagrange_key is the identifier for the lagrange multiplier
@ -223,9 +223,9 @@ public:
NonlinearConstraint2( NonlinearConstraint2(
Vector (*g)(const Config& config), Vector (*g)(const Config& config),
const std::string& key1, const std::string& key1,
Matrix (*gradG1)(const Config& config), Matrix (*G1)(const Config& config),
const std::string& key2, const std::string& key2,
Matrix (*gradG2)(const Config& config), Matrix (*G2)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -234,7 +234,7 @@ public:
* Basic constructor with direct function objects * Basic constructor with direct function objects
* Use boost.bind to construct the function objects * Use boost.bind to construct the function objects
* @param key is the identifier for the variable constrained * @param key is the identifier for the variable constrained
* @param gradG gives the gradient of the constraint function * @param G gives the jacobian of the constraint function
* @param g is the constraint function * @param g is the constraint function
* @param dim_constraint is the size of the constraint (p) * @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier * @param lagrange_key is the identifier for the lagrange multiplier
@ -243,9 +243,9 @@ public:
NonlinearConstraint2( NonlinearConstraint2(
boost::function<Vector(const Config& config)> g, boost::function<Vector(const Config& config)> g,
const std::string& key1, const std::string& key1,
boost::function<Matrix(const Config& config)> gradG1, boost::function<Matrix(const Config& config)> G1,
const std::string& key2, const std::string& key2,
boost::function<Matrix(const Config& config)> gradG2, boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);

14
cpp/testNonlinearConstraint.cpp
View File

@ -27,7 +27,7 @@ namespace test1 {
return Vector_(1, x * x - 5); return Vector_(1, x * x - 5);
} }
/** p = 1, gradG(x) = 2x */ /** p = 1, jacobianG(x) = 2x */
Matrix G(const VectorConfig& config, const list<string>& keys) { Matrix G(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x); return Matrix_(1, 1, 2 * x);
@ -110,13 +110,13 @@ namespace test2 {
return Vector_(1, x * x - 5.0 - y); return Vector_(1, x * x - 5.0 - y);
} }
/** gradient for x, gradG(x,y) in x: 2x*/ /** jacobian for x, jacobianG(x,y) in x: 2x*/
Matrix G1(const VectorConfig& config, const list<string>& keys) { Matrix G1(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](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 */ /** jacobian for y, jacobianG(x,y) in y: -1 */
Matrix G2(const VectorConfig& config, const list<string>& keys) { Matrix G2(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.back()](0); double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0); return Matrix_(1, 1, -1.0);
@ -211,7 +211,7 @@ namespace inequality1 {
return Vector_(1, g); // return the actual cost return Vector_(1, g); // return the actual cost
} }
/** p = 1, gradG(x) = 2*x */ /** p = 1, jacobianG(x) = 2*x */
Matrix G(const VectorConfig& config, const list<string>& keys) { Matrix G(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x); return Matrix_(1, 1, 2 * x);
@ -289,7 +289,7 @@ namespace binding1 {
return Vector_(1, g); // return the actual cost return Vector_(1, g); // return the actual cost
} }
/** p = 1, gradG(x) = 2*x */ /** p = 1, jacobianG(x) = 2*x */
Matrix G(double coeff, const VectorConfig& config, Matrix G(double coeff, const VectorConfig& config,
const list<string>& keys) { const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
@ -332,13 +332,13 @@ namespace binding2 {
return Vector_(1, x * x - r - y); return Vector_(1, x * x - r - y);
} }
/** gradient for x, gradG(x,y) in x: 2x*/ /** jacobian for x, jacobianG(x,y) in x: 2x*/
Matrix G1(double c, const VectorConfig& config, const list<string>& keys) { Matrix G1(double c, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, c * x); return Matrix_(1, 1, c * x);
} }
/** gradient for y, gradG(x,y) in y: -1 */ /** jacobian for y, jacobianG(x,y) in y: -1 */
Matrix G2(double c, const VectorConfig& config, const list<string>& keys) { Matrix G2(double c, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.back()](0); double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0 * c); return Matrix_(1, 1, -1.0 * c);

10
cpp/testSQP.cpp
View File

@ -324,12 +324,12 @@ namespace sqp_test1 {
return config[keys.front()] - config[keys.back()]; return config[keys.front()] - config[keys.back()];
} }
/** gradient at l1 */ /** jacobian at l1 */
Matrix G1(const VectorConfig& config, const list<string>& keys) { Matrix G1(const VectorConfig& config, const list<string>& keys) {
return eye(2); return eye(2);
} }
/** gradient at l2 */ /** jacobian at l2 */
Matrix G2(const VectorConfig& config, const list<string>& keys) { Matrix G2(const VectorConfig& config, const list<string>& keys) {
return -1 * eye(2); return -1 * eye(2);
} }
@ -344,7 +344,7 @@ namespace sqp_test2 {
return config[keys.front()] - Vector_(2, 1.0, 1.0); return config[keys.front()] - Vector_(2, 1.0, 1.0);
} }
/** gradient at x1 */ /** jacobian at x1 */
Matrix G(const VectorConfig& config, const list<string>& keys) { Matrix G(const VectorConfig& config, const list<string>& keys) {
return eye(2); return eye(2);
} }
@ -633,12 +633,12 @@ namespace sqp_stereo {
- config.landmarkPoint(getNum(keys.back())).vector(); - config.landmarkPoint(getNum(keys.back())).vector();
} }
/** gradient at l1 */ /** jacobian at l1 */
Matrix G1(const VSLAMConfig& config, const list<string>& keys) { Matrix G1(const VSLAMConfig& config, const list<string>& keys) {
return eye(3); return eye(3);
} }
/** gradient at l2 */ /** jacobian at l2 */
Matrix G2(const VSLAMConfig& config, const list<string>& keys) { Matrix G2(const VSLAMConfig& config, const list<string>& keys) {
return -1.0 * eye(3); return -1.0 * eye(3);
} }

42
cpp/testSQPOptimizer.cpp
View File

@ -60,13 +60,13 @@ Vector g_func(const VectorConfig& config, const list<string>& keys) {
return config[keys.front()]-config[keys.back()]; return config[keys.front()]-config[keys.back()];
} }
/** gradient at l1 */ /** jacobian at l1 */
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) { Matrix jac_g1(const VectorConfig& config, const list<string>& keys) {
return eye(2); return eye(2);
} }
/** gradient at l2 */ /** jacobian at l2 */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) { Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
return -1*eye(2); return -1*eye(2);
} }
} // \namespace sqp_LinearMapWarp2 } // \namespace sqp_LinearMapWarp2
@ -78,8 +78,8 @@ Vector g_func(const VectorConfig& config, const list<string>& keys) {
return config[keys.front()]-Vector_(2, 1.0, 1.0); return config[keys.front()]-Vector_(2, 1.0, 1.0);
} }
/** gradient at x1 */ /** jacobian at x1 */
Matrix grad_g(const VectorConfig& config, const list<string>& keys) { Matrix jac_g(const VectorConfig& config, const list<string>& keys) {
return eye(2); return eye(2);
} }
} // \namespace sqp_LinearMapWarp12 } // \namespace sqp_LinearMapWarp12
@ -96,7 +96,7 @@ NLGraph linearMapWarpGraph() {
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1( boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>( new NonlinearConstraint1<VectorConfig>(
boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx), boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx),
"x1", boost::bind(sqp_LinearMapWarp1::grad_g, _1, keyx), "x1", boost::bind(sqp_LinearMapWarp1::jac_g, _1, keyx),
2, "L_x1")); 2, "L_x1"));
// measurement from x1 to l1 // measurement from x1 to l1
@ -114,8 +114,8 @@ NLGraph linearMapWarpGraph() {
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>( new NonlinearConstraint2<VectorConfig>(
boost::bind(sqp_LinearMapWarp2::g_func, _1, keys), boost::bind(sqp_LinearMapWarp2::g_func, _1, keys),
"l1", boost::bind(sqp_LinearMapWarp2::grad_g1, _1, keys), "l1", boost::bind(sqp_LinearMapWarp2::jac_g1, _1, keys),
"l2", boost::bind(sqp_LinearMapWarp2::grad_g2, _1, keys), "l2", boost::bind(sqp_LinearMapWarp2::jac_g2, _1, keys),
2, "L_l1l2")); 2, "L_l1l2"));
// construct the graph // construct the graph
@ -237,15 +237,15 @@ Vector g_func(const VectorConfig& config, const list<string>& keys) {
return Vector_(1, dist2-thresh); return Vector_(1, dist2-thresh);
} }
/** gradient at pose */ /** jacobian at pose */
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) { Matrix jac_g1(const VectorConfig& config, const list<string>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()]; Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = 2.0*(x2-obs); Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1)); return Matrix_(1,2, grad(0), grad(1));
} }
/** gradient at obstacle */ /** jacobian at obstacle */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) { Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()]; Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = -2.0*(x2-obs); Vector grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1)); return Matrix_(1,2, grad(0), grad(1));
@ -276,8 +276,8 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
// the obstacle // the obstacle
list<string> keys; keys += "x2", "obs"; list<string> keys; keys += "x2", "obs";
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys), shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys),
"x2", boost::bind(sqp_avoid1::grad_g1, _1, keys), "x2", boost::bind(sqp_avoid1::jac_g1, _1, keys),
"obs",boost::bind(sqp_avoid1::grad_g2, _1, keys), "obs",boost::bind(sqp_avoid1::jac_g2, _1, keys),
1, "L_x2obs", false)); 1, "L_x2obs", false));
// construct the graph // construct the graph
@ -367,15 +367,15 @@ Vector g_func(double radius, const VectorConfig& config, const list<string>& key
return Vector_(1, dist2-thresh); return Vector_(1, dist2-thresh);
} }
/** gradient at pose */ /** jacobian at pose */
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) { Matrix jac_g1(const VectorConfig& config, const list<string>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()]; Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = 2.0*(x2-obs); Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1)); return Matrix_(1,2, grad(0), grad(1));
} }
/** gradient at obstacle */ /** jacobian at obstacle */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) { Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()]; Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = -2.0*(x2-obs); Vector grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1)); return Matrix_(1,2, grad(0), grad(1));
@ -408,8 +408,8 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
// the obstacle // the obstacle
list<string> keys; keys += "x2", "obs"; list<string> keys; keys += "x2", "obs";
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys), shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys),
"x2", boost::bind(sqp_avoid2::grad_g1, _1, keys), "x2", boost::bind(sqp_avoid2::jac_g1, _1, keys),
"obs", boost::bind(sqp_avoid2::grad_g2, _1, keys), "obs", boost::bind(sqp_avoid2::jac_g2, _1, keys),
1, "L_x2obs", false)); 1, "L_x2obs", false));
// construct the graph // construct the graph