From ae0e51aaff548c44d09fc58451cc0efda9f6e458 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Thu, 14 Oct 2010 16:08:16 +0000 Subject: [PATCH] updates for cityslam, unfinished.. --- linear/Errors.cpp | 27 +++++++++++++++ linear/Errors.h | 10 ++++++ linear/GaussianFactorGraph.cpp | 14 ++++---- linear/GaussianFactorGraph.h | 12 +++---- linear/SubgraphPreconditioner.cpp | 57 +++++++++++++++++++++---------- linear/VectorValues.h | 16 ++++++++- linear/iterative-inl.h | 4 +-- 7 files changed, 106 insertions(+), 34 deletions(-) diff --git a/linear/Errors.cpp b/linear/Errors.cpp index 526c1873a..911f78b1d 100644 --- a/linear/Errors.cpp +++ b/linear/Errors.cpp @@ -25,6 +25,18 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +Errors::Errors(){} + +/* ************************************************************************* */ +Errors::Errors(const VectorValues &V) { + this->resize(V.size()) ; + int i = 0 ; + BOOST_FOREACH( Vector &e, *this) { + e = V[i++]; + } +} + /* ************************************************************************* */ void Errors::print(const std::string& s) const { odprintf("%s:\n", s.c_str()); @@ -47,6 +59,21 @@ bool Errors::equals(const Errors& expected, double tol) const { // TODO: use boost::bind(&equal_with_abs_tol,_1, _2,tol) } +/* ************************************************************************* */ +Errors Errors::operator+(const Errors& b) const { +#ifndef NDEBUG + size_t m = size(); + if (b.size()!=m) + throw(std::invalid_argument("Errors::operator+: incompatible sizes")); +#endif + Errors result; + Errors::const_iterator it = b.begin(); + BOOST_FOREACH(const Vector& ai, *this) + result.push_back(ai + *(it++)); + return result; +} + + /* ************************************************************************* */ Errors Errors::operator-(const Errors& b) const { #ifndef NDEBUG diff --git a/linear/Errors.h b/linear/Errors.h index 6513a0a94..f683e91f0 100644 --- a/linear/Errors.h +++ b/linear/Errors.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { @@ -31,15 +32,24 @@ namespace gtsam { public: + Errors() ; + + /** break V into pieces according to its start indices */ + Errors(const VectorValues &V) ; + /** print */ void print(const std::string& s = "Errors") const; /** equals, for unit testing */ bool equals(const Errors& expected, double tol=1e-9) const; + /** Addition */ + Errors operator+(const Errors& b) const; + /** subtraction */ Errors operator-(const Errors& b) const; + }; // Errors /** diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index f6ec2e955..aabe8cfcf 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -127,13 +127,13 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, Ai->transposeMultiplyAdd(alpha,*(ei++),x); } -///* ************************************************************************* */ -//VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const { -// // It is crucial for performance to make a zero-valued clone of x -// VectorValues g = VectorValues::zero(x); -// transposeMultiplyAdd(1.0, errors(x), g); -// return g; -//} +/* ************************************************************************* */ +VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const { + // It is crucial for performance to make a zero-valued clone of x + VectorValues g = VectorValues::zero(x); + transposeMultiplyAdd(1.0, errors(x), g); + return g; +} ///* ************************************************************************* */ //set GaussianFactorGraph::find_separator(Index key) const diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index c0fa2fff5..8db893b90 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -116,12 +116,12 @@ namespace gtsam { /** x += alpha*A'*e */ void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const; -// /** -// * Calculate Gradient of A^(A*x-b) for a given config -// * @param x: VectorValues specifying where to calculate gradient -// * @return gradient, as a VectorValues as well -// */ -// VectorValues gradient(const VectorValues& x) const; + /** + * Calculate Gradient of A^(A*x-b) for a given config + * @param x: VectorValues specifying where to calculate gradient + * @return gradient, as a VectorValues as well + */ + VectorValues gradient(const VectorValues& x) const; /** Unnormalized probability. O(n) */ double probPrime(const VectorValues& c) const { diff --git a/linear/SubgraphPreconditioner.cpp b/linear/SubgraphPreconditioner.cpp index f917a6fd3..806a2f8ad 100644 --- a/linear/SubgraphPreconditioner.cpp +++ b/linear/SubgraphPreconditioner.cpp @@ -44,13 +44,22 @@ namespace gtsam { /* ************************************************************************* */ double SubgraphPreconditioner::error(const VectorValues& y) const { - Errors e; +// Errors e; +// +// // Use BayesNet order to add y contributions in order +// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { +// const Symbol& j = cg->key(); +// e.push_back(y[j]); // append y +// } +// +// // Add A2 contribution +// VectorValues x = this->x(y); +// Errors e2 = Ab2_->errors(x); +// e.splice(e.end(), e2); +// +// return 0.5 * dot(e, e); - // Use BayesNet order to add y contributions in order - BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { - const Symbol& j = cg->key(); - e.push_back(y[j]); // append y - } + Errors e(y); // Add A2 contribution VectorValues x = this->x(y); @@ -75,13 +84,15 @@ namespace gtsam { // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { - Errors e; +// Errors e; +// +// // Use BayesNet order to add y contributions in order +// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { +// const Symbol& j = cg->key(); +// e.push_back(y[j]); // append y +// } - // Use BayesNet order to add y contributions in order - BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { - const Symbol& j = cg->key(); - e.push_back(y[j]); // append y - } + Errors e(y); // Add A2 contribution VectorValues x = y; // TODO avoid ? @@ -96,13 +107,23 @@ namespace gtsam { // In-place version that overwrites e void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { - Errors::iterator ei = e.begin(); +// Errors::iterator ei = e.begin(); +// +// // Use BayesNet order to add y contributions in order +// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { +// const Symbol& j = cg->key(); +// *ei = y[j]; // append y +// ei++; +// } +// +// // Add A2 contribution +// VectorValues x = y; // TODO avoid ? +// gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y +// Ab2_->multiplyInPlace(x,ei); // use iterator version - // Use BayesNet order to add y contributions in order - BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { - const Symbol& j = cg->key(); - *ei = y[j]; // append y - ei++; + Errors::iterator ei = e.begin(); + for ( int i = 0 ; i < y.size() ; ++i, ++ei ) { + *ei = y[i]; } // Add A2 contribution diff --git a/linear/VectorValues.h b/linear/VectorValues.h index 8f4d65b3b..3e1bdc086 100644 --- a/linear/VectorValues.h +++ b/linear/VectorValues.h @@ -145,10 +145,17 @@ public: VectorValues result; result.varStarts_ = varStarts_; result.values_ = boost::numeric::ublas::project(values_, boost::numeric::ublas::range(0, varStarts_.back())) + - boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back())); + boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back())); return result; } + void operator+=(const VectorValues& c) { + assert(varStarts_ == c.varStarts_); + this->values_ = boost::numeric::ublas::project(this->values_, boost::numeric::ublas::range(0, varStarts_.back())) + + boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back())); + } + + /** * Iterator (handles both iterator and const_iterator depending on whether * the template type is const. @@ -177,11 +184,18 @@ public: value_type operator*() { return config_[curVariable_]; } }; + static VectorValues zero(const VectorValues& x) { + VectorValues cloned(x); + cloned.makeZero(); + return cloned; + } + protected: void checkVariable(Index variable) const { assert(variable < varStarts_.size()-1); } public: + friend size_t dim(const VectorValues& V) { return V.varStarts_.back(); } friend double dot(const VectorValues& V1, const VectorValues& V2) { return gtsam::dot(V1.values_, V2.values_) ; } friend void scal(double alpha, VectorValues& x) { gtsam::scal(alpha, x.values_) ; } friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { gtsam::axpy(alpha, x.values_, y.values_) ; } diff --git a/linear/iterative-inl.h b/linear/iterative-inl.h index 670a259bd..a90c235a6 100644 --- a/linear/iterative-inl.h +++ b/linear/iterative-inl.h @@ -43,8 +43,8 @@ namespace gtsam { k = 0; verbose = verb; steepest = steep; - maxIterations = (maxIt > 0) ? maxIt : x.dim() * (steepest ? 10 : 1); - reset = (size_t) (sqrt(x.dim()) + 0.5); // when to reset + maxIterations = (maxIt > 0) ? maxIt : dim(x) * (steepest ? 10 : 1); + reset = (size_t) (sqrt(dim(x)) + 0.5); // when to reset // Start with g0 = A'*(A*x0-b), d0 = - g0 // i.e., first step is in direction of negative gradient