diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp new file mode 100644 index 000000000..cce8eea9d --- /dev/null +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -0,0 +1,150 @@ +///* ---------------------------------------------------------------------------- +// +// * GTSAM Copyright 2010, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * Authors: Frank Dellaert, et al. (see THANKS for the full author list) +// +// * See LICENSE for the license information +// +// * -------------------------------------------------------------------------- */ +// +///** +// * @file SubgraphPreconditioner.cpp +// * @date Dec 31, 2009 +// * @author: Frank Dellaert +// */ +// +//#include +//#include +//#include +//#include +// +//using namespace std; +// +//namespace gtsam { +// +// /* ************************************************************************* */ +// SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, +// const sharedBayesNet& Rc1, const sharedValues& xbar) : +// Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(gaussianErrors_(*Ab2_,*xbar)) { +// } +// +// /* ************************************************************************* */ +// // x = xbar + inv(R1)*y +// VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { +//#ifdef VECTORBTREE +// if (!y.cloned(*xbar_)) throw +// invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar"); +//#endif +// VectorValues x = y; +// backSubstituteInPlace(*Rc1_,x); +// x += *xbar_; +// return x; +// } +// +//// SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const { +//// SubgraphPreconditioner result = *this ; +//// result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ; +//// return result ; +//// } +// +// /* ************************************************************************* */ +// double error(const SubgraphPreconditioner& sp, const VectorValues& y) { +// +// Errors e(y); +// VectorValues x = sp.x(y); +// Errors e2 = gaussianErrors(*sp.Ab2(),x); +// return 0.5 * (dot(e, e) + dot(e2,e2)); +// } +// +// /* ************************************************************************* */ +// // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), +// VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) { +// VectorValues x = sp.x(y); // x = inv(R1)*y +// Errors e2 = gaussianErrors(*sp.Ab2(),x); +// VectorValues gx2 = VectorValues::Zero(y); +// gtsam::transposeMultiplyAdd(*sp.Ab2(),1.0,e2,gx2); // A2'*e2; +// VectorValues gy2 = gtsam::backSubstituteTranspose(*sp.Rc1(), gx2); // inv(R1')*gx2 +// return y + gy2; +// } +// +// /* ************************************************************************* */ +// // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] +// Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y) { +// +// Errors e(y); +// +// // Add A2 contribution +// VectorValues x = y; // TODO avoid ? +// gtsam::backSubstituteInPlace(*sp.Rc1(), x); // x=inv(R1)*y +// Errors e2 = *sp.Ab2() * x; // A2*x +// e.splice(e.end(), e2); +// +// return e; +// } +// +// /* ************************************************************************* */ +// // In-place version that overwrites e +// void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e) { +// +// +// Errors::iterator ei = e.begin(); +// for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) { +// *ei = y[i]; +// } +// +// // Add A2 contribution +// VectorValues x = y; // TODO avoid ? +// gtsam::backSubstituteInPlace(*sp.Rc1(), x); // x=inv(R1)*y +// gtsam::multiplyInPlace(*sp.Ab2(),x,ei); // use iterator version +// } +// +// /* ************************************************************************* */ +// // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 +// VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e) { +// +// Errors::const_iterator it = e.begin(); +// VectorValues y = sp.zero(); +// for ( Index i = 0 ; i < y.size() ; ++i, ++it ) +// y[i] = *it ; +// sp.transposeMultiplyAdd2(1.0,it,e.end(),y); +// return y; +// } +// +// /* ************************************************************************* */ +// // y += alpha*A'*e +// void transposeMultiplyAdd +// (const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y) { +// +// +// Errors::const_iterator it = e.begin(); +// for ( Index i = 0 ; i < y.size() ; ++i, ++it ) { +// const Vector& ei = *it; +// axpy(alpha,ei,y[i]); +// } +// sp.transposeMultiplyAdd2(alpha,it,e.end(),y); +// } +// +// /* ************************************************************************* */ +// // y += alpha*inv(R1')*A2'*e2 +// void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, +// Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const { +// +// // create e2 with what's left of e +// // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? +// Errors e2; +// while (it != end) +// e2.push_back(*(it++)); +// +// VectorValues x = VectorValues::Zero(y); // x = 0 +// gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2 +// axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x +// } +// +// /* ************************************************************************* */ +// void SubgraphPreconditioner::print(const std::string& s) const { +// cout << s << endl; +// Ab2_->print(); +// } +//} // nsamespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h new file mode 100644 index 000000000..2cc760bfa --- /dev/null +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -0,0 +1,116 @@ +///* ---------------------------------------------------------------------------- +// +// * GTSAM Copyright 2010, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * Authors: Frank Dellaert, et al. (see THANKS for the full author list) +// +// * See LICENSE for the license information +// +// * -------------------------------------------------------------------------- */ +// +///** +// * @file SubgraphPreconditioner.h +// * @date Dec 31, 2009 +// * @author Frank Dellaert +// */ +// +//#pragma once +// +//#include +//#include +//#include // FIXME shouldn't have nonlinear things in linear +// +//namespace gtsam { +// +// /** +// * Subgraph conditioner class, as explained in the RSS 2010 submission. +// * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 +// * We solve R1*x=c1, and make the substitution y=R1*x-c1. +// * To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2. +// * Then solve for yhat using CG, and solve for xhat = system.x(yhat). +// */ +// class SubgraphPreconditioner { +// +// public: +// typedef boost::shared_ptr sharedBayesNet; +// typedef boost::shared_ptr > sharedFG; +// typedef boost::shared_ptr sharedValues; +// typedef boost::shared_ptr sharedErrors; +// +// private: +// sharedFG Ab1_, Ab2_; +// sharedBayesNet Rc1_; +// sharedValues xbar_; +// sharedErrors b2bar_; /** b2 - A2*xbar */ +// +// public: +// +// SubgraphPreconditioner(); +// /** +// * Constructor +// * @param Ab1: the Graph A1*x=b1 +// * @param Ab2: the Graph A2*x=b2 +// * @param Rc1: the Bayes Net R1*x=c1 +// * @param xbar: the solution to R1*x=c1 +// */ +// SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); +// +// /** Access Ab1 */ +// const sharedFG& Ab1() const { return Ab1_; } +// +// /** Access Ab2 */ +// const sharedFG& Ab2() const { return Ab2_; } +// +// /** Access Rc1 */ +// const sharedBayesNet& Rc1() const { return Rc1_; } +// +// /** +// * 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; +// +// /* A zero VectorValues with the structure of xbar */ +// VectorValues zero() const { +// VectorValues V(VectorValues::Zero(*xbar_)) ; +// return V ; +// } +// +// /** +// * Add constraint part of the error only, used in both calls above +// * y += alpha*inv(R1')*A2'*e2 +// * Takes a range indicating e2 !!!! +// */ +// void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, +// Errors::const_iterator end, VectorValues& y) const; +// +// /** print the object */ +// void print(const std::string& s = "SubgraphPreconditioner") const; +// }; +// +// /* error, given y */ +// double error(const SubgraphPreconditioner& sp, const VectorValues& y); +// +// /** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */ +// VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y); +// +// /** Apply operator A */ +// Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y); +// +// /** Apply operator A in place: needs e allocated already */ +// void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e); +// +// /** Apply operator A' */ +// VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e); +// +// /** +// * Add A'*e to y +// * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] +// */ +// void transposeMultiplyAdd(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y); +// +//} // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h new file mode 100644 index 000000000..d2dc7cee2 --- /dev/null +++ b/gtsam/linear/SubgraphSolver-inl.h @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#pragma once + +#include + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +template +void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { + + GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); + GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); + + if (parameters_->verbosity()) cout << "split the graph ..."; + split(pairs_, *graph, *Ab1, *Ab2) ; + if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; + + // // 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 + // theta_bar_ = composePoses (T_, tree, theta0[root]); + + LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!! + SubgraphPreconditioner::sharedBayesNet Rc1 = + EliminationTree::Create(sacrificialAb1)->eliminate(&EliminateQR); + SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); + + pc_ = boost::make_shared( + Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); +} + +template +VectorValues::shared_ptr SubgraphSolver::optimize() const { + + // preconditioned conjugate gradient + VectorValues zeros = pc_->zero(); + VectorValues ybar = conjugateGradients + (*pc_, zeros, *parameters_); + + boost::shared_ptr xbar = boost::make_shared() ; + *xbar = pc_->x(ybar); + return xbar; +} + +template +void SubgraphSolver::initialize(const GRAPH& G, const Values& theta0) { + // generate spanning tree + PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); + + // make the ordering + list keys = predecessorMap2Keys(tree_); + ordering_ = boost::make_shared(list(keys.begin(), keys.end())); + + // build factor pairs + pairs_.clear(); + typedef pair EG ; + BOOST_FOREACH( const EG &eg, tree_ ) { + Key key1 = Key(eg.first), + key2 = Key(eg.second) ; + pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; + } +} + +} // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp new file mode 100644 index 000000000..609b427f2 --- /dev/null +++ b/gtsam/linear/SubgraphSolver.cpp @@ -0,0 +1,50 @@ +///* ---------------------------------------------------------------------------- +// +// * GTSAM Copyright 2010, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * Authors: Frank Dellaert, et al. (see THANKS for the full author list) +// +// * See LICENSE for the license information +// +// * -------------------------------------------------------------------------- */ +// +//#include +// +//using namespace std; +// +//namespace gtsam { +// +///* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */ +//bool split(const std::map &M, +// const GaussianFactorGraph &Ab, +// GaussianFactorGraph &Ab1, +// GaussianFactorGraph &Ab2) { +// +// Ab1 = GaussianFactorGraph(); +// Ab2 = GaussianFactorGraph(); +// +// for ( size_t i = 0 ; i < Ab.size() ; ++i ) { +// +// boost::shared_ptr factor = Ab[i] ; +// +// if (factor->keys().size() > 2) +// throw(invalid_argument("split: only support factors with at most two keys")); +// if (factor->keys().size() == 1) { +// Ab1.push_back(factor); +// Ab2.push_back(factor); +// continue; +// } +// Index key1 = factor->keys()[0]; +// Index key2 = factor->keys()[1]; +// +// if ((M.find(key1) != M.end() && M.find(key1)->second == key2) || +// (M.find(key2) != M.end() && M.find(key2)->second == key1)) +// Ab1.push_back(factor); +// else +// Ab2.push_back(factor); +// } +// return true ; +//} +// +//} // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h new file mode 100644 index 000000000..f4fa34427 --- /dev/null +++ b/gtsam/linear/SubgraphSolver.h @@ -0,0 +1,100 @@ +///* ---------------------------------------------------------------------------- +// +// * GTSAM Copyright 2010, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * Authors: Frank Dellaert, et al. (see THANKS for the full author list) +// +// * See LICENSE for the license information +// +// * -------------------------------------------------------------------------- */ +// +//#pragma once +// +//#include +// +//#include +//#include +//#include +// +//namespace gtsam { +// +///* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */ +//bool split(const std::map &M, +// const GaussianFactorGraph &Ab, +// GaussianFactorGraph &Ab1, +// GaussianFactorGraph &Ab2); +// +///** +// * A nonlinear system solver using subgraph preconditioning conjugate gradient +// * Concept NonLinearSolver implements +// * linearize: G * T -> L +// * solve : L -> VectorValues +// */ +//template +//class SubgraphSolver : public IterativeSolver { +// +//private: +// typedef typename VALUES::Key Key; +// typedef typename GRAPH::Pose Pose; +// typedef typename GRAPH::Constraint Constraint; +// +// typedef boost::shared_ptr shared_ptr ; +// typedef boost::shared_ptr shared_ordering ; +// typedef boost::shared_ptr shared_graph ; +// typedef boost::shared_ptr shared_linear ; +// typedef boost::shared_ptr shared_values ; +// typedef boost::shared_ptr shared_preconditioner ; +// typedef std::map mapPairIndex ; +// +// /* the ordering derived from the spanning tree */ +// shared_ordering ordering_; +// +// /* the indice of two vertices in the gaussian factor graph */ +// mapPairIndex pairs_; +// +// /* preconditioner */ +// shared_preconditioner pc_; +// +// /* flag for direct solver - either QR or LDL */ +// bool useQR_; +// +//public: +// +// SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters ¶meters = Parameters(), bool useQR = false): +// IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); } +// +// SubgraphSolver(const LINEAR& GFG) { +// std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl; +// throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); +// } +// +// SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr& structure, bool useQR = false) { +// std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl; +// throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported"); +// } +// +// SubgraphSolver(const SubgraphSolver& solver) : +// IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {} +// +// SubgraphSolver(shared_ordering ordering, +// mapPairIndex pairs, +// shared_preconditioner pc, +// sharedParameters parameters = boost::make_shared(), +// bool useQR = true) : +// IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {} +// +// void replaceFactors(const typename LINEAR::shared_ptr &graph); +// VectorValues::shared_ptr optimize() ; +// shared_ordering ordering() const { return ordering_; } +// +//protected: +// void initialize(const GRAPH& G, const VALUES& theta0); +// +//private: +// SubgraphSolver():IterativeSolver(){} +//}; +// +//} // namespace gtsam +// +//#include