disable old subgraph preconditioners temporarily to remove name conflict
Yong-Dian Jian
parent
0a842cf0ec
commit
bc7293a0a7
|
|
@ -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
|
|
||||||
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) {
|
// // x = xbar + inv(R1)*y
|
||||||
|
// VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
|
||||||
Errors e(y);
|
//#ifdef VECTORBTREE
|
||||||
VectorValues x = sp.x(y);
|
// if (!y.cloned(*xbar_)) throw
|
||||||
Errors e2 = gaussianErrors(*sp.Ab2(),x);
|
// invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar");
|
||||||
return 0.5 * (dot(e, e) + dot(e2,e2));
|
//#endif
|
||||||
}
|
// VectorValues x = y;
|
||||||
|
// backSubstituteInPlace(*Rc1_,x);
|
||||||
/* ************************************************************************* */
|
// x += *xbar_;
|
||||||
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
|
// return x;
|
||||||
VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) {
|
// }
|
||||||
VectorValues x = sp.x(y); // x = inv(R1)*y
|
//
|
||||||
Errors e2 = gaussianErrors(*sp.Ab2(),x);
|
//// SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const {
|
||||||
VectorValues gx2 = VectorValues::Zero(y);
|
//// SubgraphPreconditioner result = *this ;
|
||||||
gtsam::transposeMultiplyAdd(*sp.Ab2(),1.0,e2,gx2); // A2'*e2;
|
//// result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ;
|
||||||
VectorValues gy2 = gtsam::backSubstituteTranspose(*sp.Rc1(), gx2); // inv(R1')*gx2
|
//// return result ;
|
||||||
return y + gy2;
|
//// }
|
||||||
}
|
//
|
||||||
|
// /* ************************************************************************* */
|
||||||
/* ************************************************************************* */
|
// double error(const SubgraphPreconditioner& sp, const VectorValues& 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 e(y);
|
||||||
|
// VectorValues x = sp.x(y);
|
||||||
Errors e(y);
|
// Errors e2 = gaussianErrors(*sp.Ab2(),x);
|
||||||
|
// return 0.5 * (dot(e, e) + dot(e2,e2));
|
||||||
// Add A2 contribution
|
// }
|
||||||
VectorValues x = y; // TODO avoid ?
|
//
|
||||||
gtsam::backSubstituteInPlace(*sp.Rc1(), x); // x=inv(R1)*y
|
// /* ************************************************************************* */
|
||||||
Errors e2 = *sp.Ab2() * x; // A2*x
|
// // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
|
||||||
e.splice(e.end(), e2);
|
// VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) {
|
||||||
|
// VectorValues x = sp.x(y); // x = inv(R1)*y
|
||||||
return e;
|
// 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
|
||||||
// In-place version that overwrites e
|
// return y + gy2;
|
||||||
void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e) {
|
// }
|
||||||
|
//
|
||||||
|
// /* ************************************************************************* */
|
||||||
Errors::iterator ei = e.begin();
|
// // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
|
||||||
for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) {
|
// Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y) {
|
||||||
*ei = y[i];
|
//
|
||||||
}
|
// 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
|
||||||
gtsam::multiplyInPlace(*sp.Ab2(),x,ei); // use iterator version
|
// Errors e2 = *sp.Ab2() * x; // A2*x
|
||||||
}
|
// e.splice(e.end(), e2);
|
||||||
|
//
|
||||||
/* ************************************************************************* */
|
// return e;
|
||||||
// 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();
|
// // In-place version that overwrites e
|
||||||
VectorValues y = sp.zero();
|
// void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e) {
|
||||||
for ( Index i = 0 ; i < y.size() ; ++i, ++it )
|
//
|
||||||
y[i] = *it ;
|
//
|
||||||
sp.transposeMultiplyAdd2(1.0,it,e.end(),y);
|
// Errors::iterator ei = e.begin();
|
||||||
return y;
|
// for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) {
|
||||||
}
|
// *ei = y[i];
|
||||||
|
// }
|
||||||
/* ************************************************************************* */
|
//
|
||||||
// y += alpha*A'*e
|
// // Add A2 contribution
|
||||||
void transposeMultiplyAdd
|
// VectorValues x = y; // TODO avoid ?
|
||||||
(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y) {
|
// gtsam::backSubstituteInPlace(*sp.Rc1(), x); // x=inv(R1)*y
|
||||||
|
// gtsam::multiplyInPlace(*sp.Ab2(),x,ei); // use iterator version
|
||||||
|
// }
|
||||||
Errors::const_iterator it = e.begin();
|
//
|
||||||
for ( Index i = 0 ; i < y.size() ; ++i, ++it ) {
|
// /* ************************************************************************* */
|
||||||
const Vector& ei = *it;
|
// // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
|
||||||
axpy(alpha,ei,y[i]);
|
// VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e) {
|
||||||
}
|
//
|
||||||
sp.transposeMultiplyAdd2(alpha,it,e.end(),y);
|
// Errors::const_iterator it = e.begin();
|
||||||
}
|
// VectorValues y = sp.zero();
|
||||||
|
// for ( Index i = 0 ; i < y.size() ; ++i, ++it )
|
||||||
/* ************************************************************************* */
|
// y[i] = *it ;
|
||||||
// y += alpha*inv(R1')*A2'*e2
|
// sp.transposeMultiplyAdd2(1.0,it,e.end(),y);
|
||||||
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
|
// return y;
|
||||||
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 ?
|
// // y += alpha*A'*e
|
||||||
Errors e2;
|
// void transposeMultiplyAdd
|
||||||
while (it != end)
|
// (const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y) {
|
||||||
e2.push_back(*(it++));
|
//
|
||||||
|
//
|
||||||
VectorValues x = VectorValues::Zero(y); // x = 0
|
// Errors::const_iterator it = e.begin();
|
||||||
gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2
|
// for ( Index i = 0 ; i < y.size() ; ++i, ++it ) {
|
||||||
axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x
|
// const Vector& ei = *it;
|
||||||
}
|
// axpy(alpha,ei,y[i]);
|
||||||
|
// }
|
||||||
/* ************************************************************************* */
|
// sp.transposeMultiplyAdd2(alpha,it,e.end(),y);
|
||||||
void SubgraphPreconditioner::print(const std::string& s) const {
|
// }
|
||||||
cout << s << endl;
|
//
|
||||||
Ab2_->print();
|
// /* ************************************************************************* */
|
||||||
}
|
// // y += alpha*inv(R1')*A2'*e2
|
||||||
} // nsamespace gtsam
|
// 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
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
|
||||||
|
|
@ -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 ¶meters = Parameters(), bool useQR = false):
|
// SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters ¶meters = 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>
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue