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

Skeleton code for GaussianFactorGraph::multiplyHessian

release/4.3a0
Pablo Fernandez Alcantarilla 2013-10-24 15:52:32 +00:00
parent 876edb4197
commit 6a383799d7
8 changed files with 74 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/linear/GaussianFactor.h
View File

@ -106,6 +106,9 @@ namespace gtsam {
*/
virtual GaussianFactor::shared_ptr negate() const = 0;
/// y += alpha * A'*A*x
virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y)=0;
private:
/** Serialization function */
friend class boost::serialization::access;

24
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -246,6 +246,14 @@ namespace gtsam {
return e;
}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::multiplyHessian(const VectorValues& x) const {
VectorValues y;
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
f->multiplyHessianAdd(1.0,x,y);
return y;
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
multiplyInPlace(x, e.begin());
@ -275,15 +283,15 @@ namespace gtsam {
/* ************************************************************************* */
// x += alpha*A'*e
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const
{
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha, *(ei++), x);
}
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
VectorValues& x) const {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha, *(ei++), x);
}
}
///* ************************************************************************* */
//void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {

3
gtsam/linear/GaussianFactorGraph.h
View File

@ -269,6 +269,9 @@ namespace gtsam {
///** return A*x */
Errors operator*(const VectorValues& x) const;
///** return A'A*x */
VectorValues multiplyHessian(const VectorValues& x) const;
///** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const VectorValues& x, Errors& e) const;

6
gtsam/linear/HessianFactor.cpp
View File

@ -499,6 +499,12 @@ GaussianFactor::shared_ptr HessianFactor::negate() const
return result;
}
/* ************************************************************************* */
void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) {
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)

4
gtsam/linear/HessianFactor.h
View File

@ -356,8 +356,8 @@ namespace gtsam {
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
/** Return A'A*x */
// Vector operator*(const VectorValues& x) const;
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y);
/**
* Densely partially eliminate with Cholesky factorization. JacobianFactors are

6
gtsam/linear/JacobianFactor.cpp
View File

@ -432,6 +432,12 @@ namespace gtsam {
return model_ ? model_->whiten(Ax) : Ax;
}
/* ************************************************************************* */
void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) {
}
/* ************************************************************************* */
void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
VectorValues& x) const

3
gtsam/linear/JacobianFactor.h
View File

@ -263,6 +263,9 @@ namespace gtsam {
/** Return A*x */
Vector operator*(const VectorValues& x) const;
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y);
/** x += A'*e. If x is initially missing any values, they are created and assumed to start as
* zero vectors. */
void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;

35
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
View File

@ -220,6 +220,41 @@ TEST(GaussianFactorGraph, eliminate_empty )
EXPECT(assert_equal(*remainingGFG, expectedLF));
}
/* ************************************************************************* */
//TEST( GaussianFactorGraph, multiplyHessian )
//{
// // A is :
// // x1 x2 x3 x4 x5
// // 1 2 3 0 0
// // 5 6 7 0 0
// // 9 10 0 11 12
// // 0 0 0 14 15
// GaussianFactorGraph A = createSimpleGaussianFactorGraph();
//
// VectorValues x = map_list_of
// (0, (Vec(2) << 1,2))
// (1, (Vec(2) << 3,4))
// (2, (Vec(1) << 5));
//
// // AtA is :
// // x1 x2 x3 x4 x5
// // 107 122 38 99 108
// // 122 140 48 110 120
// // 38 48 58 0 0
// // 99 110 0 317 342
// // 108 120 0 342 369
//
// // AtAx is:
// // 1401 1586 308 3297 3561
// VectorValues expected;
// expected.insert(0, (Vec(2) << 1401,1586));
// expected.insert(1, (Vec(2) << 308,3297));
// expected.insert(2, (Vec(1) << 3561));
//
// VectorValues actual = A.multiplyHessian(x);
// EXPECT(assert_equal(expected, actual));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */