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,
bool 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
// TODO:should do something smart to find a unique one
@ -74,7 +74,7 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
const std::string& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
gradG_(gradG), key_(key)
G_(gradG), key_(key)
{
// set a good lagrange key here
// 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);
// construct the gradient
Matrix grad = gradG_(config);
Matrix grad = G_(config);
// construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad);
@ -140,14 +140,14 @@ template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2(
Vector (*g)(const Config& config),
const std::string& key1,
Matrix (*gradG1)(const Config& config),
Matrix (*G1)(const Config& config),
const std::string& key2,
Matrix (*gradG2)(const Config& config),
Matrix (*G2)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key,
bool 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)
{
// set a good lagrange key here
@ -163,14 +163,14 @@ template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2(
boost::function<Vector(const Config& config)> g,
const std::string& key1,
boost::function<Matrix(const Config& config)> gradG1,
boost::function<Matrix(const Config& config)> G1,
const std::string& key2,
boost::function<Matrix(const Config& config)> gradG2,
boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint,
const std::string& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
gradG1_(gradG1), gradG2_(gradG2),
G1_(G1), G2_(G2),
key1_(key1), key2_(key2)
{
// set a good lagrange key here
@ -214,8 +214,8 @@ std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConst
Vector g = g_(config);
// construct the gradients
Matrix grad1 = gradG1_(config);
Matrix grad2 = gradG2_(config);
Matrix grad1 = G1_(config);
Matrix grad2 = G2_(config);
// construct probabilistic factor
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>
class NonlinearConstraint1 : public NonlinearConstraint<Config> {
@ -123,13 +123,13 @@ class NonlinearConstraint1 : public NonlinearConstraint<Config> {
private:
/**
* Calculates the gradient of the constraint function
* Calculates the jacobian of the constraint function
* returns a pxn matrix
* Use boost.bind to create the function object
* @param config to use for linearization
* @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 */
std::string key_;
@ -139,7 +139,7 @@ public:
/**
* Basic constructor
* @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 dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
@ -148,7 +148,7 @@ public:
NonlinearConstraint1(
Vector (*g)(const Config& config),
const std::string& key,
Matrix (*gradG)(const Config& config),
Matrix (*G)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key="",
bool isEquality=true);
@ -156,7 +156,7 @@ public:
/**
* Basic constructor with boost function pointers
* @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 dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
@ -165,7 +165,7 @@ public:
NonlinearConstraint1(
boost::function<Vector(const Config& config)> g,
const std::string& key,
boost::function<Matrix(const Config& config)> gradG,
boost::function<Matrix(const Config& config)> G,
size_t dim_constraint,
const std::string& lagrange_key="",
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>
class NonlinearConstraint2 : public NonlinearConstraint<Config> {
@ -196,14 +196,14 @@ class NonlinearConstraint2 : public NonlinearConstraint<Config> {
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
* returns a pxn matrix
* @param config to use for linearization
* @return the jacobian of the constraint in terms of key
*/
boost::function<Matrix(const Config& config)> gradG1_;
boost::function<Matrix(const Config& config)> gradG2_;
boost::function<Matrix(const Config& config)> G1_;
boost::function<Matrix(const Config& config)> G2_;
/** keys for the constrained variables */
std::string key1_;
@ -214,7 +214,7 @@ public:
/**
* Basic constructor
* @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 dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
@ -223,9 +223,9 @@ public:
NonlinearConstraint2(
Vector (*g)(const Config& config),
const std::string& key1,
Matrix (*gradG1)(const Config& config),
Matrix (*G1)(const Config& config),
const std::string& key2,
Matrix (*gradG2)(const Config& config),
Matrix (*G2)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key="",
bool isEquality=true);
@ -234,7 +234,7 @@ public:
* Basic constructor with direct function objects
* Use boost.bind to construct the function objects
* @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 dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
@ -243,9 +243,9 @@ public:
NonlinearConstraint2(
boost::function<Vector(const Config& config)> g,
const std::string& key1,
boost::function<Matrix(const Config& config)> gradG1,
boost::function<Matrix(const Config& config)> G1,
const std::string& key2,
boost::function<Matrix(const Config& config)> gradG2,
boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint,
const std::string& lagrange_key="",
bool isEquality=true);

14
cpp/testNonlinearConstraint.cpp
View File

@ -27,7 +27,7 @@ namespace test1 {
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) {
double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x);
@ -110,13 +110,13 @@ namespace test2 {
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) {
double x = config[keys.front()](0);
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) {
double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0);
@ -211,7 +211,7 @@ namespace inequality1 {
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) {
double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x);
@ -289,7 +289,7 @@ namespace binding1 {
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,
const list<string>& keys) {
double x = config[keys.front()](0);
@ -332,13 +332,13 @@ namespace binding2 {
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) {
double x = config[keys.front()](0);
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) {
double x = config[keys.back()](0);
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()];
}
/** gradient at l1 */
/** jacobian at l1 */
Matrix G1(const VectorConfig& config, const list<string>& keys) {
return eye(2);
}
/** gradient at l2 */
/** jacobian at l2 */
Matrix G2(const VectorConfig& config, const list<string>& keys) {
return -1 * eye(2);
}
@ -344,7 +344,7 @@ namespace sqp_test2 {
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) {
return eye(2);
}
@ -633,12 +633,12 @@ namespace sqp_stereo {
- config.landmarkPoint(getNum(keys.back())).vector();
}
/** gradient at l1 */
/** jacobian at l1 */
Matrix G1(const VSLAMConfig& config, const list<string>& keys) {
return eye(3);
}
/** gradient at l2 */
/** jacobian at l2 */
Matrix G2(const VSLAMConfig& config, const list<string>& keys) {
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()];
}
/** gradient at l1 */
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
/** jacobian at l1 */
Matrix jac_g1(const VectorConfig& config, const list<string>& keys) {
return eye(2);
}
/** gradient at l2 */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
/** jacobian at l2 */
Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
return -1*eye(2);
}
} // \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);
}
/** gradient at x1 */
Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
/** jacobian at x1 */
Matrix jac_g(const VectorConfig& config, const list<string>& keys) {
return eye(2);
}
} // \namespace sqp_LinearMapWarp12
@ -96,7 +96,7 @@ NLGraph linearMapWarpGraph() {
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>(
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"));
// measurement from x1 to l1
@ -114,8 +114,8 @@ NLGraph linearMapWarpGraph() {
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>(
boost::bind(sqp_LinearMapWarp2::g_func, _1, keys),
"l1", boost::bind(sqp_LinearMapWarp2::grad_g1, _1, keys),
"l2", boost::bind(sqp_LinearMapWarp2::grad_g2, _1, keys),
"l1", boost::bind(sqp_LinearMapWarp2::jac_g1, _1, keys),
"l2", boost::bind(sqp_LinearMapWarp2::jac_g2, _1, keys),
2, "L_l1l2"));
// construct the graph
@ -237,15 +237,15 @@ Vector g_func(const VectorConfig& config, const list<string>& keys) {
return Vector_(1, dist2-thresh);
}
/** gradient at pose */
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
/** jacobian at pose */
Matrix jac_g1(const VectorConfig& config, const list<string>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
}
/** gradient at obstacle */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
/** jacobian at obstacle */
Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
@ -276,8 +276,8 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
// the obstacle
list<string> keys; keys += "x2", "obs";
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys),
"x2", boost::bind(sqp_avoid1::grad_g1, _1, keys),
"obs",boost::bind(sqp_avoid1::grad_g2, _1, keys),
"x2", boost::bind(sqp_avoid1::jac_g1, _1, keys),
"obs",boost::bind(sqp_avoid1::jac_g2, _1, keys),
1, "L_x2obs", false));
// construct the graph
@ -367,15 +367,15 @@ Vector g_func(double radius, const VectorConfig& config, const list<string>& key
return Vector_(1, dist2-thresh);
}
/** gradient at pose */
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
/** jacobian at pose */
Matrix jac_g1(const VectorConfig& config, const list<string>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
}
/** gradient at obstacle */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
/** jacobian at obstacle */
Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
@ -408,8 +408,8 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
// the obstacle
list<string> keys; keys += "x2", "obs";
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys),
"x2", boost::bind(sqp_avoid2::grad_g1, _1, keys),
"obs", boost::bind(sqp_avoid2::grad_g2, _1, keys),
"x2", boost::bind(sqp_avoid2::jac_g1, _1, keys),
"obs", boost::bind(sqp_avoid2::jac_g2, _1, keys),
1, "L_x2obs", false));
// construct the graph