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.
							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