diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index c233c431b..42ea56e68 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -9,6 +9,7 @@ //#include #include //#include +#include #include #include //#include @@ -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 R(ptr, d, d); + Eigen::Map b(dst, d, 1); + R.triangularView().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 R(ptr, d, d); + Eigen::Map b(dst, d, 1); + R.transpose().triangularView().solveInPlace(b); + + dst += d; + ptr += sz; + } +} + +/***************************************************************************************/ +void BlockJacobiPreconditioner::build( + const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) +{ + const size_t n = keyInfo.size(); + dims_ = keyInfo.colSpec(); + + /* prepare the buffer of block diagonals */ + std::vector 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(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(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 createPreconditioner(const boost::shared_ptr parameters) { - DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters); - if ( dummy ) { + if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters) ) { return boost::make_shared(); } + else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast(parameters) ) { + return boost::make_shared(); + } + // BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast(parameters); // if ( jacobi ) { diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 5ca5ec70b..17c12446f 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -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 &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 &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 &lambda + ) ; + +protected: + + void clean() ; + + std::vector dims_; + double *buffer_; + size_t bufferSize_; + size_t nnz_; +}; + +/*********************************************************************************************/ /* factory method to create preconditioners */ boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters); diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index ea9b5ec6c..ef74ad1ef 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -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().solve(b); + EXPECT(assert_equal(x, xhat, 1e-5)); + + /* test in-place back substitution */ + xhat = b; + Rhat.triangularView().solveInPlace(xhat); + EXPECT(assert_equal(x, xhat, 1e-5)); + + /* test triangular matrix map */ + Eigen::Map Radapter(Rvector.data(), 3, 3); + xhat = Radapter.transpose().triangularView().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(); + pcg->preconditioner_ = boost::make_shared(); + 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;