diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp deleted file mode 100644 index cce8eea9d..000000000 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ /dev/null @@ -1,150 +0,0 @@ -///* ---------------------------------------------------------------------------- -// -// * 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 deleted file mode 100644 index 2cc760bfa..000000000 --- a/gtsam/linear/SubgraphPreconditioner.h +++ /dev/null @@ -1,116 +0,0 @@ -///* ---------------------------------------------------------------------------- -// -// * 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 deleted file mode 100644 index d2dc7cee2..000000000 --- a/gtsam/linear/SubgraphSolver-inl.h +++ /dev/null @@ -1,82 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 deleted file mode 100644 index 609b427f2..000000000 --- a/gtsam/linear/SubgraphSolver.cpp +++ /dev/null @@ -1,50 +0,0 @@ -///* ---------------------------------------------------------------------------- -// -// * 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 deleted file mode 100644 index f4fa34427..000000000 --- a/gtsam/linear/SubgraphSolver.h +++ /dev/null @@ -1,100 +0,0 @@ -///* ---------------------------------------------------------------------------- -// -// * 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