diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index b41b0c1d7..cce8eea9d 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -1,150 +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 +///* ---------------------------------------------------------------------------- +// +// * 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 index 1e7e6e972..2cc760bfa 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -1,116 +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 +///* ---------------------------------------------------------------------------- +// +// * 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.cpp b/gtsam/linear/SubgraphSolver.cpp index ce0934ffe..609b427f2 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -1,50 +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 +///* ---------------------------------------------------------------------------- +// +// * 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 index 9e0f0a9d7..f4fa34427 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -1,100 +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 +///* ---------------------------------------------------------------------------- +// +// * 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