GaussianFactor::transposeMultiplyAdd
							parent
							
								
									4f998e5ecd
								
							
						
					
					
						commit
						ac870bce4c
					
				|  | @ -210,6 +210,18 @@ VectorConfig GaussianFactor::operator^(const Vector& e) const { | |||
| 	return x; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void GaussianFactor::transposeMultiplyAdd(double alpha, const Vector& e, | ||||
| 		VectorConfig& x) const { | ||||
| 	Vector E = alpha * model_->whiten(e); | ||||
| 	// Just iterate over all A matrices and insert Ai^e into VectorConfig
 | ||||
| 	FOREACH_PAIR(j, Aj, As_) | ||||
| 	{ | ||||
| 		Vector& Xj = x.getReference(*j); | ||||
| 		gtsam::transposeMultiplyAdd(*Aj, E, Xj); | ||||
| 	} | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */   | ||||
| pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight) const { | ||||
| 
 | ||||
|  |  | |||
|  | @ -197,16 +197,15 @@ public: | |||
| 	void tally_separator(const Symbol& key, | ||||
| 			std::set<Symbol>& separator) const; | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * Return A*x | ||||
| 	 */ | ||||
| 	/** Return A*x */ | ||||
| 	Vector operator*(const VectorConfig& x) const; | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * Return A^x | ||||
| 	 */ | ||||
| 	/** Return A'*e */ | ||||
| 	VectorConfig operator^(const Vector& e) const; | ||||
| 
 | ||||
| 	/** x += A'*e */ | ||||
| 	void transposeMultiplyAdd(double alpha, const Vector& e, VectorConfig& x) const; | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * Return (dense) matrix associated with factor | ||||
| 	 * @param ordering of variables needed for matrix column order | ||||
|  |  | |||
|  | @ -91,9 +91,9 @@ VectorConfig GaussianFactorGraph::operator^(const Errors& e) const { | |||
| void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, | ||||
| 		VectorConfig& x) const { | ||||
| 	// For each factor add the gradient contribution
 | ||||
| 	Errors::const_iterator it = e.begin(); | ||||
| 	Errors::const_iterator ei = e.begin(); | ||||
| 	BOOST_FOREACH(sharedFactor Ai,factors_) | ||||
| 		x += (*Ai)^(alpha*(*(it++))); | ||||
| 		Ai->transposeMultiplyAdd(alpha,*(ei++),x); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -46,24 +46,24 @@ namespace gtsam { | |||
| 			return A_ ^ (A_ * x - b_); | ||||
| 		} | ||||
| 
 | ||||
| 		/** Apply operator A_ */ | ||||
| 		/** Apply operator A */ | ||||
| 		inline Vector operator*(const Vector& x) const { | ||||
| 			return A_ * x; | ||||
| 		} | ||||
| 
 | ||||
| 		/** Apply operator A_ in place*/ | ||||
| 		/** Apply operator A in place */ | ||||
| 		inline void multiplyInPlace(const Vector& x, Vector& e) const { | ||||
| 			e = A_ * x; | ||||
| 		} | ||||
| 
 | ||||
| 		/** Apply operator A_^T */ | ||||
| 		/** Apply operator A'*e */ | ||||
| 		inline Vector operator^(const Vector& e) const { | ||||
| 			return A_ ^ e; | ||||
| 		} | ||||
| 
 | ||||
| 		/** x += alpha* A_^T */ | ||||
| 		/** x += alpha* A'*e */ | ||||
| 		inline void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const { | ||||
| 			x += alpha * A_ ^ e; | ||||
| 			gtsam::transposeMultiplyAdd(A_,alpha*e,x); | ||||
| 		} | ||||
| 
 | ||||
| 		/**
 | ||||
|  |  | |||
|  | @ -68,6 +68,14 @@ TEST( GaussianFactor, operators ) | |||
| 	expectedX.insert("x1",Vector_(2,-2000.,-4000.)); | ||||
| 	expectedX.insert("x2",Vector_(2, 2000., 4000.)); | ||||
| 	CHECK(assert_equal(expectedX,lf^e)); | ||||
| 
 | ||||
| 	// test transposeMultiplyAdd
 | ||||
| 	VectorConfig x; | ||||
| 	x.insert("x1",Vector_(2, 1.,2.)); | ||||
| 	x.insert("x2",Vector_(2, 3.,4.)); | ||||
| 	VectorConfig expectedX2 = x + 0.1 * (lf^e); | ||||
| 	lf.transposeMultiplyAdd(0.1,e,x); | ||||
| 	CHECK(assert_equal(expectedX2,x)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue