From e9b93fe007e4a104458efa2a9a6ebcf8819295f7 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Mon, 18 Oct 2010 19:29:16 +0000 Subject: [PATCH] Fix the subgraph preconditioner solver --- linear/SubgraphPreconditioner.cpp | 22 ++++++++++------------ linear/SubgraphPreconditioner.h | 6 ++++++ linear/SubgraphSolver-inl.h | 26 +++++++++++++------------- linear/SubgraphSolver.h | 27 +++++++++++++++++---------- 4 files changed, 46 insertions(+), 35 deletions(-) diff --git a/linear/SubgraphPreconditioner.cpp b/linear/SubgraphPreconditioner.cpp index 08521ca7b..f2afafe95 100644 --- a/linear/SubgraphPreconditioner.cpp +++ b/linear/SubgraphPreconditioner.cpp @@ -41,30 +41,31 @@ namespace gtsam { return x; } + SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const { + SubgraphPreconditioner result = *this ; + result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ; + return result ; + } + /* ************************************************************************* */ double SubgraphPreconditioner::error(const VectorValues& y) const { // 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); - Errors e(y); - // Add A2 contribution + Errors e(y); VectorValues x = this->x(y); Errors e2 = Ab2_->errors(x); - return 0.5 * (dot(e, e) + dot(e2,e2)); } @@ -107,21 +108,19 @@ namespace gtsam { void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { // 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 Errors::iterator ei = e.begin(); - for ( int i = 0 ; i < y.size() ; ++i, ++ei ) { + for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) { *ei = y[i]; } @@ -152,7 +151,7 @@ namespace gtsam { Errors::const_iterator it = e.begin(); VectorValues y = zero(); - for ( int i = 0 ; i < y.size() ; ++i, ++it ) + for ( Index i = 0 ; i < y.size() ; ++i, ++it ) y[i] = *it ; transposeMultiplyAdd2(1.0,it,e.end(),y); return y; @@ -170,12 +169,11 @@ namespace gtsam { // const Vector& ej = *(it++); // axpy(alpha,ej,y[j]); // } -// // // get A2 part // transposeMultiplyAdd2(alpha,it,e.end(),y); Errors::const_iterator it = e.begin(); - for ( int i = 0 ; i < y.size() ; ++i, ++it ) { + for ( Index i = 0 ; i < y.size() ; ++i, ++it ) { const Vector& ei = *it; axpy(alpha,ei,y[i]) ; } diff --git a/linear/SubgraphPreconditioner.h b/linear/SubgraphPreconditioner.h index 395c1c4a6..d49225c77 100644 --- a/linear/SubgraphPreconditioner.h +++ b/linear/SubgraphPreconditioner.h @@ -63,6 +63,12 @@ namespace gtsam { // Vector b2() const { return Ab2_->rhsVector(); } // VectorValues assembleValues(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleValues(v, ordering); } + /** + * Add zero-mean i.i.d. Gaussian prior terms to each variable + * @param sigma Standard deviation of Gaussian + */ + SubgraphPreconditioner add_priors(double sigma) const; + /* x = xbar + inv(R1)*y */ VectorValues x(const VectorValues& y) const; diff --git a/linear/SubgraphSolver-inl.h b/linear/SubgraphSolver-inl.h index b762ffe88..023aae9b9 100644 --- a/linear/SubgraphSolver-inl.h +++ b/linear/SubgraphSolver-inl.h @@ -21,9 +21,8 @@ #include #include - -#include #include +#include #include using namespace std; @@ -41,36 +40,37 @@ namespace gtsam { void SubgraphSolver::initialize(const Graph& G, const Values& theta0) { // generate spanning tree - PredecessorMap tree = G.template findMinimumSpanningTree(); - list keys = predecessorMap2Keys(tree); + PredecessorMap tree = gtsam::findMinimumSpanningTree(G); // split the graph if (verbose_) cout << "generating spanning tree and split the graph ..."; - G.template split(tree, T_, C_); + // G.template split(tree, T_, C_); + gtsam::split(G, tree, T_, C_) ; if (verbose_) cout << T_.size() << " and " << C_.size() << " factors" << endl; // make the ordering - list symbols; - symbols.resize(keys.size()); - std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol); - ordering_ = boost::shared_ptr(new Ordering(symbols)); + list keys = predecessorMap2Keys(tree); + ordering_ = boost::shared_ptr(new Ordering(list(keys.begin(), keys.end()))); + + // Add a HardConstraint to the root, otherwise the root will be singular + Key root = keys.back(); + T_.addHardConstraint(root, theta0[root]); // compose the approximate solution - Key root = keys.back(); theta_bar_ = composePoses (T_, tree, theta0[root]); } /* ************************************************************************* */ template boost::shared_ptr SubgraphSolver::linearize(const Graph& G, const Values& theta_bar) const { - SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar); - SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar); + SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar, *ordering_); + SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar, *ordering_); #ifdef TIMING SubgraphPreconditioner::sharedBayesNet Rc1; SubgraphPreconditioner::sharedValues xbar; #else GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!! - SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_); + SubgraphPreconditioner::sharedBayesNet Rc1 = Inference::Eliminate(sacrificialAb1) ; SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); #endif // TODO: there does not seem to be a good reason to have Ab1_ diff --git a/linear/SubgraphSolver.h b/linear/SubgraphSolver.h index f0ed0eb0c..3512e1615 100644 --- a/linear/SubgraphSolver.h +++ b/linear/SubgraphSolver.h @@ -19,8 +19,8 @@ #include #include -#include #include +#include namespace gtsam { @@ -67,20 +67,27 @@ namespace gtsam { */ boost::shared_ptr linearize(const Graph& G, const Values& theta_bar) const; - /** - * solve for the optimal displacement in the tangent space, and then solve - * the resulted linear system - */ - VectorValues optimize(SubgraphPreconditioner& system) const; - boost::shared_ptr prepareLinear(const SubgraphPreconditioner& fg) const { - return boost::shared_ptr(new SubgraphSolver(*this)); - } + /** + * solve for the optimal displacement in the tangent space, and then solve + * the resulted linear system + */ + VectorValues optimize(SubgraphPreconditioner& system) const; + + boost::shared_ptr prepareLinear(const SubgraphPreconditioner& fg) const { + return boost::shared_ptr(new SubgraphSolver(*this)); + } + + + /** expmap the Values given the stored Ordering */ + Values expmap(const Values& config, const VectorValues& delta) const { + return config.expmap(delta, *ordering_); + } }; template const size_t SubgraphSolver::maxIterations_; template const bool SubgraphSolver::verbose_; template const double SubgraphSolver::epsilon_; - template const double SubgraphSolver::epsilon_abs_; + template const double SubgraphSolver::epsilon_abs_; } // nsamespace gtsam