From 3b6c4917a9ef7da0ba6d51381586be1e5ef961c0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 31 Jan 2010 04:39:41 +0000 Subject: [PATCH] GaussianBayesNet::backSubstituteInPlace --- cpp/BayesNetPreconditioner.cpp | 5 ++++- cpp/GaussianBayesNet.cpp | 19 +++++++++++++------ cpp/GaussianBayesNet.h | 5 +++++ cpp/GaussianConditional.cpp | 8 +++----- cpp/SubgraphPreconditioner.cpp | 11 ++++++++--- cpp/testGaussianBayesNet.cpp | 26 ++++++++++++++++++++++---- 6 files changed, 55 insertions(+), 19 deletions(-) diff --git a/cpp/BayesNetPreconditioner.cpp b/cpp/BayesNetPreconditioner.cpp index d3d3a006c..abe87086f 100644 --- a/cpp/BayesNetPreconditioner.cpp +++ b/cpp/BayesNetPreconditioner.cpp @@ -48,8 +48,11 @@ namespace gtsam { /* ************************************************************************* */ // In-place version that overwrites e + // TODO: version that takes scratch space for x void BayesNetPreconditioner::multiplyInPlace(const VectorConfig& y, Errors& e) const { - Ab_.multiplyInPlace(x(y),e); // TODO Avoid temporary x ? + VectorConfig x = y; + backSubstituteInPlace(Rd_,x); + Ab_.multiplyInPlace(x,e); } /* ************************************************************************* */ diff --git a/cpp/GaussianBayesNet.cpp b/cpp/GaussianBayesNet.cpp index b058b7e0d..6f3f550e1 100644 --- a/cpp/GaussianBayesNet.cpp +++ b/cpp/GaussianBayesNet.cpp @@ -78,9 +78,16 @@ boost::shared_ptr optimize_(const GaussianBayesNet& bn) } /* ************************************************************************* */ -// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) VectorConfig backSubstitute(const GaussianBayesNet& bn, const VectorConfig& y) { - VectorConfig x; + VectorConfig x = y; + backSubstituteInPlace(bn,x); + return x; +} + +/* ************************************************************************* */ +// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) +void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y) { + VectorConfig& x = y; /** solve each node in turn in topological sort order (parents first)*/ BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) { // i^th part of R*x=y, x=inv(R)*y @@ -91,12 +98,12 @@ VectorConfig backSubstitute(const GaussianBayesNet& bn, const VectorConfig& y) { for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) { const Symbol& j = it->first; const Matrix& Rij = it->second; - zi -= Rij * x[j]; + Vector& xj = x.getReference(j); + axpy(-1.0, Rij*xj, zi); // TODO: use BLAS level 2 } - Vector xi = gtsam::backSubstituteUpper(cg->get_R(), zi); - x.insert(i,xi); // store result in partial solution + Vector& xi = x.getReference(i); + xi = gtsam::backSubstituteUpper(cg->get_R(), zi); } - return x; } /* ************************************************************************* */ diff --git a/cpp/GaussianBayesNet.h b/cpp/GaussianBayesNet.h index c30599739..aacf52023 100644 --- a/cpp/GaussianBayesNet.h +++ b/cpp/GaussianBayesNet.h @@ -56,6 +56,11 @@ namespace gtsam { */ VectorConfig backSubstitute(const GaussianBayesNet& bn, const VectorConfig& y); + /* + * Backsubstitute in place, y is replaced with solution + */ + void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y); + /* * Transpose Backsubstitute * gy=inv(L)*gx by solving L*gy=gx. diff --git a/cpp/GaussianConditional.cpp b/cpp/GaussianConditional.cpp index 21d08725d..18cac8c75 100644 --- a/cpp/GaussianConditional.cpp +++ b/cpp/GaussianConditional.cpp @@ -95,14 +95,12 @@ list GaussianConditional::parents() const { /* ************************************************************************* */ Vector GaussianConditional::solve(const VectorConfig& x) const { Vector rhs = d_; - for (Parents::const_iterator it = parents_.begin(); it - != parents_.end(); it++) { + for (Parents::const_iterator it = parents_.begin(); it!= parents_.end(); it++) { const Symbol& j = it->first; const Matrix& Aj = it->second; - rhs -= Aj * x[j]; + axpy(-1, Aj * x[j], rhs); // TODO use BLAS level 2 } - Vector result = backSubstituteUpper(R_, rhs, false); - return result; + return backSubstituteUpper(R_, rhs, false); } /* ************************************************************************* */ diff --git a/cpp/SubgraphPreconditioner.cpp b/cpp/SubgraphPreconditioner.cpp index 6b8556c4f..e9c30ecb5 100644 --- a/cpp/SubgraphPreconditioner.cpp +++ b/cpp/SubgraphPreconditioner.cpp @@ -19,7 +19,10 @@ namespace gtsam { /* ************************************************************************* */ // x = xbar + inv(R1)*y VectorConfig SubgraphPreconditioner::x(const VectorConfig& y) const { - return *xbar_ + gtsam::backSubstitute(*Rc1_, y); + VectorConfig x = y; + backSubstituteInPlace(*Rc1_,x); + x += *xbar_; + return x; } /* ************************************************************************* */ @@ -63,7 +66,8 @@ namespace gtsam { } // Add A2 contribution - VectorConfig x = gtsam::backSubstitute(*Rc1_, y); // x=inv(R1)*y + VectorConfig x = y; // TODO avoid ? + gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y Errors e2 = *Ab2_ * x; // A2*x e.splice(e.end(), e2); @@ -84,7 +88,8 @@ namespace gtsam { } // Add A2 contribution - VectorConfig x = gtsam::backSubstitute(*Rc1_, y); // x=inv(R1)*y + VectorConfig x = y; // TODO avoid ? + gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y Ab2_->multiplyInPlace(x,ei); // use iterator version } diff --git a/cpp/testGaussianBayesNet.cpp b/cpp/testGaussianBayesNet.cpp index d4aacf154..1beabfec3 100644 --- a/cpp/testGaussianBayesNet.cpp +++ b/cpp/testGaussianBayesNet.cpp @@ -79,14 +79,32 @@ TEST( GaussianBayesNet, optimize ) VectorConfig actual = optimize(cbn); VectorConfig expected; - Vector x(1), y(1); - x(0) = 4; y(0) = 5; - expected.insert("x",x); - expected.insert("y",y); + expected.insert("x",Vector_(1,4.)); + expected.insert("y",Vector_(1,5.)); CHECK(assert_equal(expected,actual)); } +/* ************************************************************************* */ +TEST( GaussianBayesNet, backSubstitute ) +{ + GaussianBayesNet cbn = createSmallGaussianBayesNet(); + + VectorConfig y, expected; + y.insert("x",Vector_(1,4.)); + y.insert("y",Vector_(1,2.)); + expected.insert("x",Vector_(1,2.)); + expected.insert("y",Vector_(1,2.)); + + // test functional version + VectorConfig actual = backSubstitute(cbn,y); + CHECK(assert_equal(expected,actual)); + + // test imperative version + backSubstituteInPlace(cbn,y); + CHECK(assert_equal(expected,y)); +} + /* ************************************************************************* */ #ifdef HAVE_BOOST_SERIALIZATION TEST( GaussianBayesNet, serialize )