12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

disable old subgraph preconditioners temporarily to remove name conflict

release/4.3a0
Yong-Dian Jian 2012-02-02 23:16:45 +00:00
parent 0a842cf0ec
commit bc7293a0a7
4 changed files with 416 additions and 416 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

300
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -1,150 +1,150 @@
/* ---------------------------------------------------------------------------- ///* ----------------------------------------------------------------------------
//
* GTSAM Copyright 2010, Georgia Tech Research Corporation, // * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 // * Atlanta, Georgia 30332-0415
* All Rights Reserved // * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) // * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
//
* See LICENSE for the license information // * See LICENSE for the license information
//
* -------------------------------------------------------------------------- */ // * -------------------------------------------------------------------------- */
//
/** ///**
* @file SubgraphPreconditioner.cpp // * @file SubgraphPreconditioner.cpp
* @date Dec 31, 2009 // * @date Dec 31, 2009
* @author: Frank Dellaert // * @author: Frank Dellaert
*/ // */
//
#include <boost/foreach.hpp> //#include <boost/foreach.hpp>
#include <gtsam/linear/SubgraphPreconditioner.h> //#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/JacobianFactor.h> //#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> //#include <gtsam/linear/GaussianFactorGraph.h>
//
using namespace std; //using namespace std;
//
namespace gtsam { //namespace gtsam {
//
/* ************************************************************************* */ // /* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, // SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar) : // const sharedBayesNet& Rc1, const sharedValues& xbar) :
Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(gaussianErrors_(*Ab2_,*xbar)) { // Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(gaussianErrors_(*Ab2_,*xbar)) {
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
// x = xbar + inv(R1)*y // // x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { // VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
#ifdef VECTORBTREE //#ifdef VECTORBTREE
if (!y.cloned(*xbar_)) throw // if (!y.cloned(*xbar_)) throw
invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar"); // invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar");
#endif //#endif
VectorValues x = y; // VectorValues x = y;
backSubstituteInPlace(*Rc1_,x); // backSubstituteInPlace(*Rc1_,x);
x += *xbar_; // x += *xbar_;
return x; // return x;
} // }
//
// SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const { //// SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const {
// SubgraphPreconditioner result = *this ; //// SubgraphPreconditioner result = *this ;
// result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ; //// result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ;
// return result ; //// return result ;
// } //// }
//
/* ************************************************************************* */ // /* ************************************************************************* */
double error(const SubgraphPreconditioner& sp, const VectorValues& y) { // double error(const SubgraphPreconditioner& sp, const VectorValues& y) {
//
Errors e(y); // Errors e(y);
VectorValues x = sp.x(y); // VectorValues x = sp.x(y);
Errors e2 = gaussianErrors(*sp.Ab2(),x); // Errors e2 = gaussianErrors(*sp.Ab2(),x);
return 0.5 * (dot(e, e) + dot(e2,e2)); // return 0.5 * (dot(e, e) + dot(e2,e2));
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), // // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) { // VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) {
VectorValues x = sp.x(y); // x = inv(R1)*y // VectorValues x = sp.x(y); // x = inv(R1)*y
Errors e2 = gaussianErrors(*sp.Ab2(),x); // Errors e2 = gaussianErrors(*sp.Ab2(),x);
VectorValues gx2 = VectorValues::Zero(y); // VectorValues gx2 = VectorValues::Zero(y);
gtsam::transposeMultiplyAdd(*sp.Ab2(),1.0,e2,gx2); // A2'*e2; // gtsam::transposeMultiplyAdd(*sp.Ab2(),1.0,e2,gx2); // A2'*e2;
VectorValues gy2 = gtsam::backSubstituteTranspose(*sp.Rc1(), gx2); // inv(R1')*gx2 // VectorValues gy2 = gtsam::backSubstituteTranspose(*sp.Rc1(), gx2); // inv(R1')*gx2
return y + gy2; // return y + gy2;
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] // // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y) { // Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y) {
//
Errors e(y); // Errors e(y);
//
// Add A2 contribution // // Add A2 contribution
VectorValues x = y; // TODO avoid ? // VectorValues x = y; // TODO avoid ?
gtsam::backSubstituteInPlace(*sp.Rc1(), x); // x=inv(R1)*y // gtsam::backSubstituteInPlace(*sp.Rc1(), x); // x=inv(R1)*y
Errors e2 = *sp.Ab2() * x; // A2*x // Errors e2 = *sp.Ab2() * x; // A2*x
e.splice(e.end(), e2); // e.splice(e.end(), e2);
//
return e; // return e;
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
// In-place version that overwrites e // // In-place version that overwrites e
void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e) { // void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e) {
//
//
Errors::iterator ei = e.begin(); // Errors::iterator ei = e.begin();
for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) { // for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) {
*ei = y[i]; // *ei = y[i];
} // }
//
// Add A2 contribution // // Add A2 contribution
VectorValues x = y; // TODO avoid ? // VectorValues x = y; // TODO avoid ?
gtsam::backSubstituteInPlace(*sp.Rc1(), x); // x=inv(R1)*y // gtsam::backSubstituteInPlace(*sp.Rc1(), x); // x=inv(R1)*y
gtsam::multiplyInPlace(*sp.Ab2(),x,ei); // use iterator version // gtsam::multiplyInPlace(*sp.Ab2(),x,ei); // use iterator version
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 // // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e) { // VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e) {
//
Errors::const_iterator it = e.begin(); // Errors::const_iterator it = e.begin();
VectorValues y = sp.zero(); // VectorValues y = sp.zero();
for ( Index i = 0 ; i < y.size() ; ++i, ++it ) // for ( Index i = 0 ; i < y.size() ; ++i, ++it )
y[i] = *it ; // y[i] = *it ;
sp.transposeMultiplyAdd2(1.0,it,e.end(),y); // sp.transposeMultiplyAdd2(1.0,it,e.end(),y);
return y; // return y;
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
// y += alpha*A'*e // // y += alpha*A'*e
void transposeMultiplyAdd // void transposeMultiplyAdd
(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y) { // (const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y) {
//
//
Errors::const_iterator it = e.begin(); // Errors::const_iterator it = e.begin();
for ( Index i = 0 ; i < y.size() ; ++i, ++it ) { // for ( Index i = 0 ; i < y.size() ; ++i, ++it ) {
const Vector& ei = *it; // const Vector& ei = *it;
axpy(alpha,ei,y[i]); // axpy(alpha,ei,y[i]);
} // }
sp.transposeMultiplyAdd2(alpha,it,e.end(),y); // sp.transposeMultiplyAdd2(alpha,it,e.end(),y);
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2 // // y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, // void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const { // Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {
//
// create e2 with what's left of e // // create e2 with what's left of e
// TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? // // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
Errors e2; // Errors e2;
while (it != end) // while (it != end)
e2.push_back(*(it++)); // e2.push_back(*(it++));
//
VectorValues x = VectorValues::Zero(y); // x = 0 // VectorValues x = VectorValues::Zero(y); // x = 0
gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2 // gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2
axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x // axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
void SubgraphPreconditioner::print(const std::string& s) const { // void SubgraphPreconditioner::print(const std::string& s) const {
cout << s << endl; // cout << s << endl;
Ab2_->print(); // Ab2_->print();
} // }
} // nsamespace gtsam //} // nsamespace gtsam

232
gtsam/linear/SubgraphPreconditioner.h
View File

@ -1,116 +1,116 @@
/* ---------------------------------------------------------------------------- ///* ----------------------------------------------------------------------------
//
* GTSAM Copyright 2010, Georgia Tech Research Corporation, // * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 // * Atlanta, Georgia 30332-0415
* All Rights Reserved // * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) // * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
//
* See LICENSE for the license information // * See LICENSE for the license information
//
* -------------------------------------------------------------------------- */ // * -------------------------------------------------------------------------- */
//
/** ///**
* @file SubgraphPreconditioner.h // * @file SubgraphPreconditioner.h
* @date Dec 31, 2009 // * @date Dec 31, 2009
* @author Frank Dellaert // * @author Frank Dellaert
*/ // */
//
#pragma once //#pragma once
//
#include <gtsam/linear/JacobianFactor.h> //#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h> //#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/nonlinear/Ordering.h> // FIXME shouldn't have nonlinear things in linear //#include <gtsam/nonlinear/Ordering.h> // FIXME shouldn't have nonlinear things in linear
//
namespace gtsam { //namespace gtsam {
//
/** // /**
* Subgraph conditioner class, as explained in the RSS 2010 submission. // * 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 // * 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. // * 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. // * 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). // * Then solve for yhat using CG, and solve for xhat = system.x(yhat).
*/ // */
class SubgraphPreconditioner { // class SubgraphPreconditioner {
//
public: // public:
typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet; // typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
typedef boost::shared_ptr<const FactorGraph<JacobianFactor> > sharedFG; // typedef boost::shared_ptr<const FactorGraph<JacobianFactor> > sharedFG;
typedef boost::shared_ptr<const VectorValues> sharedValues; // typedef boost::shared_ptr<const VectorValues> sharedValues;
typedef boost::shared_ptr<const Errors> sharedErrors; // typedef boost::shared_ptr<const Errors> sharedErrors;
//
private: // private:
sharedFG Ab1_, Ab2_; // sharedFG Ab1_, Ab2_;
sharedBayesNet Rc1_; // sharedBayesNet Rc1_;
sharedValues xbar_; // sharedValues xbar_;
sharedErrors b2bar_; /** b2 - A2*xbar */ // sharedErrors b2bar_; /** b2 - A2*xbar */
//
public: // public:
//
SubgraphPreconditioner(); // SubgraphPreconditioner();
/** // /**
* Constructor // * Constructor
* @param Ab1: the Graph A1*x=b1 // * @param Ab1: the Graph A1*x=b1
* @param Ab2: the Graph A2*x=b2 // * @param Ab2: the Graph A2*x=b2
* @param Rc1: the Bayes Net R1*x=c1 // * @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to R1*x=c1 // * @param xbar: the solution to R1*x=c1
*/ // */
SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); // SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
//
/** Access Ab1 */ // /** Access Ab1 */
const sharedFG& Ab1() const { return Ab1_; } // const sharedFG& Ab1() const { return Ab1_; }
//
/** Access Ab2 */ // /** Access Ab2 */
const sharedFG& Ab2() const { return Ab2_; } // const sharedFG& Ab2() const { return Ab2_; }
//
/** Access Rc1 */ // /** Access Rc1 */
const sharedBayesNet& Rc1() const { return Rc1_; } // const sharedBayesNet& Rc1() const { return Rc1_; }
//
/** // /**
* Add zero-mean i.i.d. Gaussian prior terms to each variable // * Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian // * @param sigma Standard deviation of Gaussian
*/ // */
// SubgraphPreconditioner add_priors(double sigma) const; //// SubgraphPreconditioner add_priors(double sigma) const;
//
/* x = xbar + inv(R1)*y */ // /* x = xbar + inv(R1)*y */
VectorValues x(const VectorValues& y) const; // VectorValues x(const VectorValues& y) const;
//
/* A zero VectorValues with the structure of xbar */ // /* A zero VectorValues with the structure of xbar */
VectorValues zero() const { // VectorValues zero() const {
VectorValues V(VectorValues::Zero(*xbar_)) ; // VectorValues V(VectorValues::Zero(*xbar_)) ;
return V ; // return V ;
} // }
//
/** // /**
* Add constraint part of the error only, used in both calls above // * Add constraint part of the error only, used in both calls above
* y += alpha*inv(R1')*A2'*e2 // * y += alpha*inv(R1')*A2'*e2
* Takes a range indicating e2 !!!! // * Takes a range indicating e2 !!!!
*/ // */
void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, // void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin,
Errors::const_iterator end, VectorValues& y) const; // Errors::const_iterator end, VectorValues& y) const;
//
/** print the object */ // /** print the object */
void print(const std::string& s = "SubgraphPreconditioner") const; // void print(const std::string& s = "SubgraphPreconditioner") const;
}; // };
//
/* error, given y */ // /* error, given y */
double error(const SubgraphPreconditioner& sp, const VectorValues& y); // double error(const SubgraphPreconditioner& sp, const VectorValues& y);
//
/** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */ // /** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */
VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y); // VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y);
//
/** Apply operator A */ // /** Apply operator A */
Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y); // Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y);
//
/** Apply operator A in place: needs e allocated already */ // /** Apply operator A in place: needs e allocated already */
void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e); // void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e);
//
/** Apply operator A' */ // /** Apply operator A' */
VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e); // VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e);
//
/** // /**
* Add A'*e to y // * Add A'*e to y
* y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] // * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2]
*/ // */
void transposeMultiplyAdd(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y); // void transposeMultiplyAdd(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y);
//
} // namespace gtsam //} // namespace gtsam

100
gtsam/linear/SubgraphSolver.cpp
View File

@ -1,50 +1,50 @@
/* ---------------------------------------------------------------------------- ///* ----------------------------------------------------------------------------
//
* GTSAM Copyright 2010, Georgia Tech Research Corporation, // * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 // * Atlanta, Georgia 30332-0415
* All Rights Reserved // * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) // * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
//
* See LICENSE for the license information // * See LICENSE for the license information
//
* -------------------------------------------------------------------------- */ // * -------------------------------------------------------------------------- */
//
#include <gtsam/linear/SubgraphSolver.h> //#include <gtsam/linear/SubgraphSolver.h>
//
using namespace std; //using namespace std;
//
namespace gtsam { //namespace gtsam {
//
/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */ ///* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
bool split(const std::map<Index, Index> &M, //bool split(const std::map<Index, Index> &M,
const GaussianFactorGraph &Ab, // const GaussianFactorGraph &Ab,
GaussianFactorGraph &Ab1, // GaussianFactorGraph &Ab1,
GaussianFactorGraph &Ab2) { // GaussianFactorGraph &Ab2) {
//
Ab1 = GaussianFactorGraph(); // Ab1 = GaussianFactorGraph();
Ab2 = GaussianFactorGraph(); // Ab2 = GaussianFactorGraph();
//
for ( size_t i = 0 ; i < Ab.size() ; ++i ) { // for ( size_t i = 0 ; i < Ab.size() ; ++i ) {
//
boost::shared_ptr<GaussianFactor> factor = Ab[i] ; // boost::shared_ptr<GaussianFactor> factor = Ab[i] ;
//
if (factor->keys().size() > 2) // if (factor->keys().size() > 2)
throw(invalid_argument("split: only support factors with at most two keys")); // throw(invalid_argument("split: only support factors with at most two keys"));
if (factor->keys().size() == 1) { // if (factor->keys().size() == 1) {
Ab1.push_back(factor); // Ab1.push_back(factor);
Ab2.push_back(factor); // Ab2.push_back(factor);
continue; // continue;
} // }
Index key1 = factor->keys()[0]; // Index key1 = factor->keys()[0];
Index key2 = factor->keys()[1]; // Index key2 = factor->keys()[1];
//
if ((M.find(key1) != M.end() && M.find(key1)->second == key2) || // if ((M.find(key1) != M.end() && M.find(key1)->second == key2) ||
(M.find(key2) != M.end() && M.find(key2)->second == key1)) // (M.find(key2) != M.end() && M.find(key2)->second == key1))
Ab1.push_back(factor); // Ab1.push_back(factor);
else // else
Ab2.push_back(factor); // Ab2.push_back(factor);
} // }
return true ; // return true ;
} //}
//
} // \namespace gtsam //} // \namespace gtsam

200
gtsam/linear/SubgraphSolver.h
View File

@ -1,100 +1,100 @@
/* ---------------------------------------------------------------------------- ///* ----------------------------------------------------------------------------
//
* GTSAM Copyright 2010, Georgia Tech Research Corporation, // * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 // * Atlanta, Georgia 30332-0415
* All Rights Reserved // * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) // * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
//
* See LICENSE for the license information // * See LICENSE for the license information
//
* -------------------------------------------------------------------------- */ // * -------------------------------------------------------------------------- */
//
#pragma once //#pragma once
//
#include <boost/make_shared.hpp> //#include <boost/make_shared.hpp>
//
#include <gtsam/linear/GaussianFactorGraph.h> //#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h> //#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h> //#include <gtsam/linear/SubgraphPreconditioner.h>
//
namespace gtsam { //namespace gtsam {
//
/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */ ///* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
bool split(const std::map<Index, Index> &M, //bool split(const std::map<Index, Index> &M,
const GaussianFactorGraph &Ab, // const GaussianFactorGraph &Ab,
GaussianFactorGraph &Ab1, // GaussianFactorGraph &Ab1,
GaussianFactorGraph &Ab2); // GaussianFactorGraph &Ab2);
//
/** ///**
* A nonlinear system solver using subgraph preconditioning conjugate gradient // * A nonlinear system solver using subgraph preconditioning conjugate gradient
* Concept NonLinearSolver<G,T,L> implements // * Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L // * linearize: G * T -> L
* solve : L -> VectorValues // * solve : L -> VectorValues
*/ // */
template<class GRAPH, class LINEAR, class VALUES> //template<class GRAPH, class LINEAR, class VALUES>
class SubgraphSolver : public IterativeSolver { //class SubgraphSolver : public IterativeSolver {
//
private: //private:
typedef typename VALUES::Key Key; // typedef typename VALUES::Key Key;
typedef typename GRAPH::Pose Pose; // typedef typename GRAPH::Pose Pose;
typedef typename GRAPH::Constraint Constraint; // typedef typename GRAPH::Constraint Constraint;
//
typedef boost::shared_ptr<const SubgraphSolver> shared_ptr ; // typedef boost::shared_ptr<const SubgraphSolver> shared_ptr ;
typedef boost::shared_ptr<Ordering> shared_ordering ; // typedef boost::shared_ptr<Ordering> shared_ordering ;
typedef boost::shared_ptr<GRAPH> shared_graph ; // typedef boost::shared_ptr<GRAPH> shared_graph ;
typedef boost::shared_ptr<LINEAR> shared_linear ; // typedef boost::shared_ptr<LINEAR> shared_linear ;
typedef boost::shared_ptr<VALUES> shared_values ; // typedef boost::shared_ptr<VALUES> shared_values ;
typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ; // typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
typedef std::map<Index,Index> mapPairIndex ; // typedef std::map<Index,Index> mapPairIndex ;
//
/* the ordering derived from the spanning tree */ // /* the ordering derived from the spanning tree */
shared_ordering ordering_; // shared_ordering ordering_;
//
/* the indice of two vertices in the gaussian factor graph */ // /* the indice of two vertices in the gaussian factor graph */
mapPairIndex pairs_; // mapPairIndex pairs_;
//
/* preconditioner */ // /* preconditioner */
shared_preconditioner pc_; // shared_preconditioner pc_;
//
/* flag for direct solver - either QR or LDL */ // /* flag for direct solver - either QR or LDL */
bool useQR_; // bool useQR_;
//
public: //public:
//
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false): // SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); } // IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
//
SubgraphSolver(const LINEAR& GFG) { // SubgraphSolver(const LINEAR& GFG) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl; // std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); // throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
} // }
//
SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure, bool useQR = false) { // SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure, bool useQR = false) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl; // std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported"); // throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported");
} // }
//
SubgraphSolver(const SubgraphSolver& solver) : // SubgraphSolver(const SubgraphSolver& solver) :
IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {} // IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {}
//
SubgraphSolver(shared_ordering ordering, // SubgraphSolver(shared_ordering ordering,
mapPairIndex pairs, // mapPairIndex pairs,
shared_preconditioner pc, // shared_preconditioner pc,
sharedParameters parameters = boost::make_shared<Parameters>(), // sharedParameters parameters = boost::make_shared<Parameters>(),
bool useQR = true) : // bool useQR = true) :
IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {} // IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {}
//
void replaceFactors(const typename LINEAR::shared_ptr &graph); // void replaceFactors(const typename LINEAR::shared_ptr &graph);
VectorValues::shared_ptr optimize() ; // VectorValues::shared_ptr optimize() ;
shared_ordering ordering() const { return ordering_; } // shared_ordering ordering() const { return ordering_; }
//
protected: //protected:
void initialize(const GRAPH& G, const VALUES& theta0); // void initialize(const GRAPH& G, const VALUES& theta0);
//
private: //private:
SubgraphSolver():IterativeSolver(){} // SubgraphSolver():IterativeSolver(){}
}; //};
//
} // namespace gtsam //} // namespace gtsam
//
#include <gtsam/linear/SubgraphSolver-inl.h> //#include <gtsam/linear/SubgraphSolver-inl.h>