add BlockJacobiPreconditioner class and unit test
Yong-Dian Jian
parent
e8d3809917
commit
67398f0f13
|
|
@ -9,6 +9,7 @@
|
|||
//#include <gtsam/linear/CombinatorialPreconditioner.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
//#include <gtsam/linear/FactorGraphUtil-inl.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/PCGSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
//#include <gtsam/linear/JacobianFactorGraph.h>
|
||||
|
|
@ -83,26 +84,137 @@ std::string PreconditionerParameters::verbosityTranslator(PreconditionerParamete
|
|||
else return "UNKNOWN";
|
||||
}
|
||||
|
||||
///***************************************************************************************/
|
||||
//void Preconditioner::replaceFactors(const JacobianFactorGraph &jfg, const double lambda)
|
||||
//{
|
||||
// const Parameters &p = *parameters_;
|
||||
//
|
||||
// if ( keyInfo_.size() == 0 ) {
|
||||
// keyInfo_ = KeyInfo(jfg, *orderingNatural(jfg));
|
||||
// }
|
||||
//
|
||||
// if ( p.verbosity() >= Parameters::COMPLEXITY )
|
||||
// cout << "Preconditioner::replaceFactors with a jfg of " << jfg.size() << " factors."<< endl;
|
||||
//}
|
||||
//
|
||||
/***************************************************************************************/
|
||||
BlockJacobiPreconditioner::BlockJacobiPreconditioner()
|
||||
: Base(), buffer_(0), bufferSize_(0), nnz_(0) {}
|
||||
|
||||
/***************************************************************************************/
|
||||
BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { clean(); }
|
||||
|
||||
/***************************************************************************************/
|
||||
void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const {
|
||||
|
||||
const size_t n = dims_.size();
|
||||
double *ptr = buffer_, *dst = x.data();
|
||||
|
||||
std::copy(y.data(), y.data() + y.rows(), x.data());
|
||||
|
||||
for ( size_t i = 0 ; i < n ; ++i ) {
|
||||
const size_t d = dims_[i];
|
||||
const size_t sz = d*d;
|
||||
|
||||
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
|
||||
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
|
||||
R.triangularView<Eigen::Upper>().solveInPlace(b);
|
||||
|
||||
dst += d;
|
||||
ptr += sz;
|
||||
}
|
||||
}
|
||||
|
||||
/***************************************************************************************/
|
||||
void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const {
|
||||
const size_t n = dims_.size();
|
||||
double *ptr = buffer_, *dst = x.data();
|
||||
|
||||
std::copy(y.data(), y.data() + y.rows(), x.data());
|
||||
|
||||
for ( size_t i = 0 ; i < n ; ++i ) {
|
||||
const size_t d = dims_[i];
|
||||
const size_t sz = d*d;
|
||||
|
||||
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
|
||||
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
|
||||
R.transpose().triangularView<Eigen::Upper>().solveInPlace(b);
|
||||
|
||||
dst += d;
|
||||
ptr += sz;
|
||||
}
|
||||
}
|
||||
|
||||
/***************************************************************************************/
|
||||
void BlockJacobiPreconditioner::build(
|
||||
const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
|
||||
{
|
||||
const size_t n = keyInfo.size();
|
||||
dims_ = keyInfo.colSpec();
|
||||
|
||||
/* prepare the buffer of block diagonals */
|
||||
std::vector<Matrix> blocks; blocks.reserve(n);
|
||||
|
||||
/* allocate memory for the factorization of block diagonals */
|
||||
size_t nnz = 0;
|
||||
for ( size_t i = 0 ; i < n ; ++i ) {
|
||||
const size_t dim = dims_[i];
|
||||
blocks.push_back(Matrix::Zero(dim, dim));
|
||||
// nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
|
||||
nnz += dim*dim;
|
||||
}
|
||||
|
||||
/* compute the block diagonal by scanning over the factors */
|
||||
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
|
||||
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
|
||||
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
|
||||
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
|
||||
const Matrix &Ai = jf->getA(it);
|
||||
blocks[entry.index()] += (Ai.transpose() * Ai);
|
||||
}
|
||||
}
|
||||
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
|
||||
for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
|
||||
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
|
||||
const Matrix &Hii = hf->info(it, it).selfadjointView();
|
||||
blocks[entry.index()] += Hii;
|
||||
}
|
||||
}
|
||||
else {
|
||||
throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
|
||||
}
|
||||
}
|
||||
|
||||
/* if necessary, allocating the memory for cacheing the factorization results */
|
||||
if ( nnz > bufferSize_ ) {
|
||||
clean();
|
||||
buffer_ = new double [nnz];
|
||||
bufferSize_ = nnz;
|
||||
}
|
||||
nnz_ = nnz;
|
||||
|
||||
/* factorizing the blocks respectively */
|
||||
double *ptr = buffer_;
|
||||
for ( size_t i = 0 ; i < n ; ++i ) {
|
||||
/* use eigen to decompose Di */
|
||||
const Matrix R = blocks[i].llt().matrixL().transpose();
|
||||
|
||||
/* store the data in the buffer */
|
||||
size_t sz = dims_[i]*dims_[i] ;
|
||||
std::copy(R.data(), R.data() + sz, ptr);
|
||||
|
||||
/* advance the pointer */
|
||||
ptr += sz;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
void BlockJacobiPreconditioner::clean() {
|
||||
if ( buffer_ ) {
|
||||
delete [] buffer_;
|
||||
buffer_ = 0;
|
||||
bufferSize_ = 0;
|
||||
nnz_ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/***************************************************************************************/
|
||||
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {
|
||||
|
||||
DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters);
|
||||
if ( dummy ) {
|
||||
if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
|
||||
return boost::make_shared<DummyPreconditioner>();
|
||||
}
|
||||
else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
|
||||
return boost::make_shared<BlockJacobiPreconditioner>();
|
||||
}
|
||||
|
||||
|
||||
// BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditioner::Parameters>(parameters);
|
||||
// if ( jacobi ) {
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@ namespace gtsam {
|
|||
class GaussianFactorGraph;
|
||||
class KeyInfo;
|
||||
class VectorValues;
|
||||
//class Subgraph;
|
||||
|
||||
/* parameters for the preconditioner */
|
||||
struct PreconditionerParameters {
|
||||
|
|
@ -75,15 +74,15 @@ public:
|
|||
|
||||
/* implement x = S^{-1} y */
|
||||
virtual void solve(const Vector& y, Vector &x) const = 0;
|
||||
virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
// virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
|
||||
/* implement x = S^{-T} y */
|
||||
virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
|
||||
virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
|
||||
/* implement x = S^{-1} S^{-T} y */
|
||||
virtual void fullSolve(const Vector& y, Vector &x) const = 0;
|
||||
virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
// /* implement x = S^{-1} S^{-T} y */
|
||||
// virtual void fullSolve(const Vector& y, Vector &x) const = 0;
|
||||
// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
|
||||
|
||||
/* build/factorize the preconditioner */
|
||||
virtual void build(
|
||||
|
|
@ -91,23 +90,6 @@ public:
|
|||
const KeyInfo &info,
|
||||
const std::map<Key,Vector> &lambda
|
||||
) = 0;
|
||||
|
||||
// /* complexity index */
|
||||
// virtual size_t complexity() const = 0;
|
||||
//
|
||||
// /* is the preconditioner dependent to data */
|
||||
// virtual bool isStatic() const = 0;
|
||||
//
|
||||
// /* is the preconditioner kind of spanning subgraph preconditioner */
|
||||
// virtual bool isSubgraph() const = 0;
|
||||
//
|
||||
// /* return A\b */
|
||||
// virtual void xstar(Vector &result) const = 0 ;
|
||||
//
|
||||
//protected:
|
||||
// Parameters::shared_ptr parameters_;
|
||||
// KeyInfo keyInfo_;
|
||||
|
||||
};
|
||||
|
||||
/*******************************************************************************************/
|
||||
|
|
@ -131,30 +113,57 @@ public:
|
|||
|
||||
/* Computation Interfaces for raw vector */
|
||||
virtual void solve(const Vector& y, Vector &x) const { x = y; }
|
||||
virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; }
|
||||
// virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; }
|
||||
|
||||
virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; }
|
||||
virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; }
|
||||
// virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; }
|
||||
|
||||
virtual void fullSolve(const Vector& y, Vector &x) const { x = y; }
|
||||
virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; }
|
||||
// virtual void fullSolve(const Vector& y, Vector &x) const { x = y; }
|
||||
// virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; }
|
||||
|
||||
virtual void build(
|
||||
const GaussianFactorGraph &gfg,
|
||||
const KeyInfo &info,
|
||||
const std::map<Key,Vector> &lambda
|
||||
) {}
|
||||
|
||||
// virtual void replaceFactors(const JacobianFactorGraph &jfg, const double lambda = 0.0) {
|
||||
// Base::replaceFactors(jfg,lambda);
|
||||
// }
|
||||
// virtual void buildPreconditioner() {}
|
||||
// virtual size_t complexity() const { return 0; }
|
||||
// virtual bool isStatic() const { return true; }
|
||||
// virtual bool isSubgraph() const { return false; }
|
||||
// virtual void xstar(Vector &result) const {}
|
||||
};
|
||||
|
||||
/*******************************************************************************************/
|
||||
struct BlockJacobiPreconditionerParameters : public PreconditionerParameters {
|
||||
typedef PreconditionerParameters Base;
|
||||
BlockJacobiPreconditionerParameters() : Base() {}
|
||||
virtual ~BlockJacobiPreconditionerParameters() {}
|
||||
};
|
||||
|
||||
/*******************************************************************************************/
|
||||
class BlockJacobiPreconditioner : public Preconditioner {
|
||||
public:
|
||||
typedef Preconditioner Base;
|
||||
BlockJacobiPreconditioner() ;
|
||||
virtual ~BlockJacobiPreconditioner() ;
|
||||
|
||||
/* Computation Interfaces for raw vector */
|
||||
virtual void solve(const Vector& y, Vector &x) const;
|
||||
virtual void transposeSolve(const Vector& y, Vector& x) const ;
|
||||
// virtual void fullSolve(const Vector& y, Vector &x) const ;
|
||||
|
||||
virtual void build(
|
||||
const GaussianFactorGraph &gfg,
|
||||
const KeyInfo &info,
|
||||
const std::map<Key,Vector> &lambda
|
||||
) ;
|
||||
|
||||
protected:
|
||||
|
||||
void clean() ;
|
||||
|
||||
std::vector<size_t> dims_;
|
||||
double *buffer_;
|
||||
size_t bufferSize_;
|
||||
size_t nnz_;
|
||||
};
|
||||
|
||||
/*********************************************************************************************/
|
||||
/* factory method to create preconditioners */
|
||||
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters);
|
||||
|
||||
|
|
|
|||
|
|
@ -50,7 +50,47 @@ using symbol_shorthand::X;
|
|||
using symbol_shorthand::L;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, optimization_method )
|
||||
TEST( PCGSolver, llt ) {
|
||||
Matrix R = (Matrix(3,3) <<
|
||||
1., -1., -1.,
|
||||
0., 2., -1.,
|
||||
0., 0., 1.);
|
||||
Matrix AtA = R.transpose() * R;
|
||||
|
||||
Vector Rvector = (Vector(9) << 1., -1., -1.,
|
||||
0., 2., -1.,
|
||||
0., 0., 1.);
|
||||
// Vector Rvector = (Vector(6) << 1., -1., -1.,
|
||||
// 2., -1.,
|
||||
// 1.);
|
||||
|
||||
Vector b = (Vector(3) << 1., 2., 3.);
|
||||
|
||||
Vector x = (Vector(3) << 6.5, 2.5, 3.) ;
|
||||
|
||||
/* test cholesky */
|
||||
Matrix Rhat = AtA.llt().matrixL().transpose();
|
||||
EXPECT(assert_equal(R, Rhat, 1e-5));
|
||||
|
||||
/* test backward substitution */
|
||||
Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(b);
|
||||
EXPECT(assert_equal(x, xhat, 1e-5));
|
||||
|
||||
/* test in-place back substitution */
|
||||
xhat = b;
|
||||
Rhat.triangularView<Eigen::Upper>().solveInPlace(xhat);
|
||||
EXPECT(assert_equal(x, xhat, 1e-5));
|
||||
|
||||
/* test triangular matrix map */
|
||||
Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3);
|
||||
xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(b);
|
||||
EXPECT(assert_equal(x, xhat, 1e-5));
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PCGSolver, dummy )
|
||||
{
|
||||
LevenbergMarquardtParams paramsPCG;
|
||||
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
|
||||
|
|
@ -69,6 +109,26 @@ TEST( NonlinearOptimizer, optimization_method )
|
|||
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PCGSolver, blockjacobi )
|
||||
{
|
||||
LevenbergMarquardtParams paramsPCG;
|
||||
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
|
||||
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
|
||||
pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>();
|
||||
paramsPCG.iterativeParams = pcg;
|
||||
|
||||
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
|
||||
|
||||
Point2 x0(10,10);
|
||||
Values c0;
|
||||
c0.insert(X(1), x0);
|
||||
|
||||
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
|
||||
|
||||
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue