diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 0505f6c01..5e7e08f61 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -2,7 +2,7 @@ * ConjugateGradientSolver.cpp * * Created on: Jun 4, 2014 - * Author: ydjian + * Author: Yong-Dian Jian */ #include diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 6e8509309..91b58c2d4 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -9,6 +9,14 @@ * -------------------------------------------------------------------------- */ +/** + * @file ConjugateGradientSolver.h + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian + * @author Sungtae An + * @date Nov 6, 2014 + **/ + #pragma once #include @@ -82,9 +90,13 @@ public: /*********************************************************************************************/ /* * A template of linear preconditioned conjugate gradient method. - * System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v, - * rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. + * + ** REFERENCES: + * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, + * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. */ template V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { @@ -93,8 +105,8 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju estimate = residual = direction = q1 = q2 = initial; system.residual(estimate, q1); /* q1 = b-Ax */ - system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ - system.rightPrecondition(residual, direction);/* d = S^{-1} r */ + system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ + system.rightPrecondition(residual, direction);/* p = L^{-T} r */ double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta; @@ -116,21 +128,21 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju if ( k % iReset == 0 ) { system.residual(estimate, q1); /* q1 = b-Ax */ - system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ - system.rightPrecondition(residual, direction); /* d = S^{-1} r */ + system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ + system.rightPrecondition(residual, direction); /* p = L^{-T} r */ currentGamma = system.dot(residual, residual); } - system.multiply(direction, q1); /* q1 = A d */ - alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */ - system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */ - system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */ - system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */ + system.multiply(direction, q1); /* q1 = A p */ + alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */ + system.axpy(alpha, direction, estimate); /* estimate += alpha * p */ + system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */ + system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */ prevGamma = currentGamma; - currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */ + currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */ beta = currentGamma / prevGamma; - system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */ + system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */ system.scal(beta, direction); - system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */ + system.axpy(1.0, q1, direction); /* p = q1 + beta * p */ if (parameters.verbosity() >= ConjugateGradientParameters::ERROR ) std::cout << "[PCG] k = " << k diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 27eb57b44..ffb744239 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -2,20 +2,14 @@ * PCGSolver.cpp * * Created on: Feb 14, 2012 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #include -//#include -//#include -//#include -//#include #include #include -//#include -//#include -//#include -//#include + #include #include #include @@ -33,6 +27,7 @@ void PCGSolverParameters::print(ostream &os) const { /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { + parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } @@ -76,97 +71,47 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { } /*****************************************************************************/ -void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const { - /* implement Ax, assume x and Ax are pre-allocated */ +void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { + /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ - /* reset y */ - Ax.setZero(); + // Build a VectorValues for Vector x + VectorValues vvX = buildVectorValues(x,keyInfo_); - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate At A x*/ - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - const Matrix Ai = jf->getA(it); - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - Ax.segment(entry.colstart(), entry.dim()) - += Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim())); - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate H x */ + // VectorValues form of A'Ax for multiplyHessianAdd + VectorValues vvAtAx; - /* use buffer to avoid excessive table lookups */ - const size_t sz = hf->size(); - vector y; - y.reserve(sz); - for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { - /* initialize y to zeros */ - y.push_back(zero(hf->getDim(it))); - } + // vvAtAx += 1.0 * A'Ax for each factor + gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); - for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) { - /* retrieve the key mapping */ - const KeyInfoEntry &entry = keyInfo_.find(*j)->second; - // xj is the input vector - const Vector xj = x.segment(entry.colstart(), entry.dim()); - size_t idx = 0; - for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) { - if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj; - else y[idx] += hf->info(i, j).knownOffDiagonal() * xj; - } - } + // Make the result as Vector form + AtAx = vvAtAx.vector(); - /* accumulate to r */ - for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) { - /* retrieve the key mapping */ - const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second; - Ax.segment(entry.colstart(), entry.dim()) += y[i]; - } - } - else { - throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } } /*****************************************************************************/ void GaussianFactorGraphSystem::getb(Vector &b) const { /* compute rhs, assume b pre-allocated */ - /* reset */ - b.setZero(); + // Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues + VectorValues vvb = gfg_.gradientAtZero(); - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - const Vector rhs = jf->getb(); - /* accumulate At rhs */ - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ; - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate g */ - for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it); - } - } - else { - throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } + // Make the result as Vector form + b = -vvb.vector(); } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.transposeSolve(x, y); } +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { + // For a preconditioner M = L*L^T + // Calculate y = L^{-1} x + preconditioner_.solve(x, y); +} /**********************************************************************************/ -void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.solve(x, y); } +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const { + // For a preconditioner M = L*L^T + // Calculate y = L^{-T} x + preconditioner_.transposeSolve(x, y); +} /**********************************************************************************/ VectorValues buildVectorValues(const Vector &v, diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 9af362fba..4c3006a3f 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -2,7 +2,8 @@ * Preconditioner.cpp * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #include @@ -94,7 +95,7 @@ void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const { const Eigen::Map R(ptr, d, d); Eigen::Map b(dst, d, 1); - R.triangularView().solveInPlace(b); + R.triangularView().solveInPlace(b); dst += d; ptr += sz; @@ -141,11 +142,9 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - std::map hessianMap = gf->hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); - } + std::map hessianMap =gfg.hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); /* if necessary, allocating the memory for cacheing the factorization results */ if ( nnz > bufferSize_ ) { @@ -159,11 +158,12 @@ void BlockJacobiPreconditioner::build( double *ptr = buffer_; for ( size_t i = 0 ; i < n ; ++i ) { /* use eigen to decompose Di */ - const Matrix R = blocks[i].llt().matrixL().transpose(); + /* It is same as L = chol(M,'lower') in MATLAB where M is full preconditioner */ + const Matrix L = blocks[i].llt().matrixL(); /* store the data in the buffer */ size_t sz = dims_[i]*dims_[i] ; - std::copy(R.data(), R.data() + sz, ptr); + std::copy(L.data(), L.data() + sz, ptr); /* advance the pointer */ ptr += sz; diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 10ceb78a9..623b29863 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -2,7 +2,8 @@ * Preconditioner.h * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #pragma once @@ -57,7 +58,8 @@ struct GTSAM_EXPORT PreconditionerParameters { }; /* PCG aims to solve the problem: A x = b by reparametrizing it as - * S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M + * L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b, + * where A \approx L L^{T}, or A \approx M * The goal of this class is to provide a general interface to all preconditioners */ class GTSAM_EXPORT Preconditioner { public: @@ -70,15 +72,15 @@ public: /* Computation Interfaces */ - /* implement x = S^{-1} y */ + /* implement x = L^{-1} y */ virtual void solve(const Vector& y, Vector &x) const = 0; // virtual void solve(const VectorValues& y, VectorValues &x) const = 0; - /* implement x = S^{-T} y */ + /* implement x = L^{-T} y */ virtual void transposeSolve(const Vector& y, Vector& x) const = 0; // virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; -// /* implement x = S^{-1} S^{-T} y */ +// /* implement x = L^{-1} L^{-T} y */ // virtual void fullSolve(const Vector& y, Vector &x) const = 0; // virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index ad2c0767a..43b4935fc 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -28,86 +28,98 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -static GaussianFactorGraph createSimpleGaussianFactorGraph() { - GaussianFactorGraph fg; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); - // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector2(2.0, -1.0), unit2); - // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector2(0.0, 1.0), unit2); - // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); - return fg; +TEST( PCGsolver, verySimpleLinearSystem) { + + // Ax = [4 1][u] = [1] x0 = [2] + // [1 3][v] [2] [1] + // + // exact solution x = [1/11, 7/11]'; + // + + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2)); + + // Exact solution already known + VectorValues exactSolution; + exactSolution.insert(0, (Vector(2) << 1./11., 7./11.).finished()); + //exactSolution.print("Exact"); + + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); + //deltaDirect.print("Direct"); + + // Solve the system using Preconditioned Conjugate Gradient solver + // Common PCG parameters + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); + + // With Dummy preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); + //deltaPCGDummy.print("PCG Dummy"); + + // With Block-Jacobi preconditioner + pcg->preconditioner_ = boost::make_shared(); + // It takes more than 1000 iterations for this test + pcg->setMaxIterations(1500); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); + + EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); } /* ************************************************************************* */ -// Copy of BlockJacobiPreconditioner::build -std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) -{ - const size_t n = keyInfo.size(); - std::vector dims_ = keyInfo.colSpec(); +TEST(PCGSolver, simpleLinearSystem) { + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + //SharedDiagonal unit2 = noiseModel::Unit::Create(2); + SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3)); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - /* prepare the buffer of block diagonals */ - std::vector blocks; blocks.reserve(n); + // Expected solution + VectorValues expectedSolution; + expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished()); + expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished()); + expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished()); + //expectedSolution.print("Expected"); - /* 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; - } + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); + //deltaDirect.print("Direct"); - /* 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."); - } - } + // Solve the system using Preconditioned Conjugate Gradient solver + // Common PCG parameters + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); - return blocks; -} + // With Dummy preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); + //deltaPCGDummy.print("PCG Dummy"); -/* ************************************************************************* */ -TEST( Preconditioner, buildBlocks ) { - // Create simple Gaussian factor graph and initial values - GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - Values initial; - initial.insert(0,Point2(4, 5)); - initial.insert(1,Point2(0, 1)); - initial.insert(2,Point2(-5, 7)); + // With Block-Jacobi preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); - // Expected Hessian block diagonal matrices - std::map expectedHessian =gfg.hessianBlockDiagonal(); - - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(gfg, KeyInfo(gfg)); - - // Compare the number of block diagonal matrices - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - - // Compare the values of matrices - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - for(; it1!=expectedHessian.end(); it1++, it2++) - EXPECT(assert_equal(it1->second, *it2)); } /* ************************************************************************* */