Added a flag to nonlinear equality to disable throwing errors if the constraint is not fulfilled.
							parent
							
								
									2bd7a1c513
								
							
						
					
					
						commit
						e73f56f817
					
				|  | @ -18,13 +18,17 @@ namespace gtsam { | |||
| 	 * Template default compare function that assumes a testable T | ||||
| 	 */ | ||||
| 	template<class T> | ||||
| 	bool compare(const T& a, const T& b) {return a.equals(b);	} | ||||
| 
 | ||||
| 	bool compare(const T& a, const T& b) { return a.equals(b); } | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * An equality factor that forces either one variable to a constant, | ||||
| 	 * or a set of variables to be equal to each other. | ||||
| 	 * Throws an error at linearization if the constraints are not met. | ||||
| 	 * | ||||
| 	 * Depending on flag, throws an error at linearization if the constraints are not met. | ||||
| 	 * | ||||
| 	 * Switchable implementation: | ||||
| 	 *   - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain | ||||
| 	 *   - ONLY_EXACT   : throws error at linearization if not at exact feasible point, and infinite error | ||||
| 	 */ | ||||
| 	template<class Config, class Key, class T> | ||||
| 	class NonlinearEquality: public NonlinearFactor1<Config, Key, T> { | ||||
|  | @ -33,6 +37,12 @@ namespace gtsam { | |||
| 		// feasible value
 | ||||
| 		T feasible_; | ||||
| 
 | ||||
| 		// error handling flag
 | ||||
| 		bool allow_error_; | ||||
| 
 | ||||
| 		// error gain in allow error case
 | ||||
| 		double error_gain_; | ||||
| 
 | ||||
| 	public: | ||||
| 
 | ||||
| 		/**
 | ||||
|  | @ -43,10 +53,21 @@ namespace gtsam { | |||
| 		typedef NonlinearFactor1<Config, Key, T> Base; | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Constructor | ||||
| 		 * Constructor - forces exact evaluation | ||||
| 		 */ | ||||
| 		NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) : | ||||
| 			Base(noiseModel::Constrained::All(dim(feasible)), j), feasible_(feasible), compare_(compare) { | ||||
| 			Base(noiseModel::Constrained::All(dim(feasible)), j), feasible_(feasible), | ||||
| 			allow_error_(false), error_gain_(std::numeric_limits<double>::infinity()), | ||||
| 			compare_(compare) { | ||||
| 		} | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Constructor - allows inexact evaluation | ||||
| 		 */ | ||||
| 		NonlinearEquality(const Key& j, const T& feasible, double error_gain, bool (*compare)(const T&, const T&) = compare<T>) : | ||||
| 			Base(noiseModel::Constrained::All(dim(feasible)), j), feasible_(feasible), | ||||
| 			allow_error_(true), error_gain_(error_gain), | ||||
| 			compare_(compare) { | ||||
| 		} | ||||
| 
 | ||||
| 		void print(const std::string& s = "") const { | ||||
|  | @ -64,10 +85,24 @@ namespace gtsam { | |||
| 			return compare_(feasible_, p->feasible_); | ||||
| 		} | ||||
| 
 | ||||
| 		/** actual error function calculation */ | ||||
| 		virtual double error(const Config& c) const { | ||||
| 			const T& xj = c[this->key_]; | ||||
| 			Vector e = this->unwhitenedError(c); | ||||
| 			if (allow_error_ || !compare_(xj, feasible_)) { | ||||
| 				return error_gain_ * inner_prod(e,e); | ||||
| 			} else { | ||||
| 				return 0.0; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/** error function */ | ||||
| 		inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H) const { | ||||
| 		inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const { | ||||
| 			size_t nj = dim(feasible_); | ||||
| 			if (compare_(feasible_,xj)) { | ||||
| 			if (allow_error_) { | ||||
| 				if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
 | ||||
| 				return logmap(xj, feasible_); | ||||
| 			} else if (compare_(feasible_,xj)) { | ||||
| 				if (H) *H = eye(nj); | ||||
| 				return zero(nj); // set error to zero if equal
 | ||||
| 			} else { | ||||
|  |  | |||
|  | @ -75,8 +75,9 @@ namespace gtsam { | |||
| 
 | ||||
| 		/**
 | ||||
| 		 * calculate the error of the factor | ||||
| 		 * Override for systems with unusual noise models | ||||
| 		 */ | ||||
| 		double error(const Config& c) const { | ||||
| 		virtual double error(const Config& c) const { | ||||
| 			return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c)); | ||||
| 		} | ||||
| 
 | ||||
|  |  | |||
|  | @ -19,6 +19,7 @@ using namespace gtsam; | |||
| 
 | ||||
| typedef NonlinearEquality<VectorConfig,string,Vector> NLE; | ||||
| typedef boost::shared_ptr<NLE> shared_nle; | ||||
| 
 | ||||
| typedef TypedSymbol<Pose2, 'x'> PoseKey; | ||||
| typedef LieConfig<PoseKey, Pose2> PoseConfig; | ||||
| typedef NonlinearEquality<PoseConfig, PoseKey, Pose2> PoseNLE; | ||||
|  | @ -139,7 +140,7 @@ TEST ( NonlinearEquality, error ) { | |||
| 	CHECK(assert_equal(actual, zero(2))); | ||||
| 
 | ||||
| 	actual = nle->unwhitenedError(bad_linearize); | ||||
| 	CHECK(assert_equal(actual, repeat(2, 1.0/0.0))); | ||||
| 	CHECK(assert_equal(actual, repeat(2, std::numeric_limits<double>::infinity()))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -159,6 +160,34 @@ TEST ( NonlinearEquality, equals ) { | |||
| 	CHECK(!nle1->equals(*nle3)); // test config
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST ( NonlinearEquality, allow_error_vector ) { | ||||
| 	Symbol key1 = "x"; | ||||
| 	Vector feasible1 = Vector_(3, 1.0, 2.0, 3.0); | ||||
| 	double error_gain = 500.0; | ||||
| 	NLE nle(key1, feasible1, error_gain,vector_compare); | ||||
| 
 | ||||
| 	// the unwhitened error should provide logmap to the feasible state
 | ||||
| 	Vector badPoint1 = Vector_(3, 0.0, 2.0, 3.0); | ||||
| 	Vector actVec = nle.evaluateError(badPoint1); | ||||
| 	Vector expVec = Vector_(3, 1.0, 0.0, 0.0); | ||||
| 	CHECK(assert_equal(expVec, actVec)); | ||||
| 
 | ||||
| 	// the actual error should have a gain on it
 | ||||
| 	VectorConfig config; | ||||
| 	config.insert(key1, badPoint1); | ||||
| 	double actError = nle.error(config); | ||||
| 	DOUBLES_EQUAL(500.0, actError, 1e-9); | ||||
| 
 | ||||
| 	// check linearization
 | ||||
| 	GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); | ||||
| 	Matrix A1 = eye(3,3); | ||||
| 	Vector b = -expVec; | ||||
| 	SharedDiagonal model = noiseModel::Constrained::All(3); | ||||
| 	GaussianFactor::shared_ptr expLinFactor(new GaussianFactor(key1, A1, b, model)); | ||||
| 	CHECK(assert_equal(*expLinFactor, *actLinFactor)); | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr); } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue