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

GaussianFactor::transposeMultiplyAdd

release/4.3a0
Frank Dellaert 2010-01-31 03:33:53 +00:00
parent 4f998e5ecd
commit ac870bce4c
5 changed files with 32 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/GaussianFactor.cpp
View File

@ -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 {

11
cpp/GaussianFactor.h
View File

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

4
cpp/GaussianFactorGraph.cpp
View File

@ -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);
}
/* ************************************************************************* */

10
cpp/iterative.h
View File

@ -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);
}
/**

8
cpp/testGaussianFactor.cpp
View File

@ -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));
}
/* ************************************************************************* */