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

Merged in featrue/fixPCG (pull request #67) 

Fixed PCG solver
release/4.3a0
Sungtae An 2014-12-11 10:36:47 -05:00
parent 22218eafc4 e9b0f7b98f
commit c1f048dc42
6 changed files with 154 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/ConjugateGradientSolver.cpp
View File

@ -2,7 +2,7 @@
* ConjugateGradientSolver.cpp * ConjugateGradientSolver.cpp
* *
* Created on: Jun 4, 2014 * Created on: Jun 4, 2014
* Author: ydjian * Author: Yong-Dian Jian
*/ */
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>

40
gtsam/linear/ConjugateGradientSolver.h
View File

@ -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 #pragma once
#include <gtsam/linear/IterativeSolver.h> #include <gtsam/linear/IterativeSolver.h>
@ -82,9 +90,13 @@ public:
/*********************************************************************************************/ /*********************************************************************************************/
/* /*
* A template of linear preconditioned conjugate gradient method. * A template of linear preconditioned conjugate gradient method.
* System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v, * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
* rightPrecondition(v, S^{-1}v), 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. * 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 <class S, class V> template <class S, class V>
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) { V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) {
@ -93,8 +105,8 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju
estimate = residual = direction = q1 = q2 = initial; estimate = residual = direction = q1 = q2 = initial;
system.residual(estimate, q1); /* q1 = b-Ax */ system.residual(estimate, q1); /* q1 = b-Ax */
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
system.rightPrecondition(residual, direction);/* d = S^{-1} r */ system.rightPrecondition(residual, direction);/* p = L^{-T} r */
double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta; 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 ) { if ( k % iReset == 0 ) {
system.residual(estimate, q1); /* q1 = b-Ax */ system.residual(estimate, q1); /* q1 = b-Ax */
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
system.rightPrecondition(residual, direction); /* d = S^{-1} r */ system.rightPrecondition(residual, direction); /* p = L^{-T} r */
currentGamma = system.dot(residual, residual); currentGamma = system.dot(residual, residual);
} }
system.multiply(direction, q1); /* q1 = A d */ system.multiply(direction, q1); /* q1 = A p */
alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */ alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */
system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */ system.axpy(alpha, direction, estimate); /* estimate += alpha * p */
system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */ system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */
system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */ system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */
prevGamma = currentGamma; prevGamma = currentGamma;
currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */ currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */
beta = currentGamma / prevGamma; beta = currentGamma / prevGamma;
system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */ system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */
system.scal(beta, direction); 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 ) if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
std::cout << "[PCG] k = " << k std::cout << "[PCG] k = " << k

111
gtsam/linear/PCGSolver.cpp
View File

@ -2,20 +2,14 @@
* PCGSolver.cpp * PCGSolver.cpp
* *
* Created on: Feb 14, 2012 * Created on: Feb 14, 2012
* Author: ydjian * Author: Yong-Dian Jian
* Author: Sungtae An
*/ */
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
//#include <gtsam/inference/FactorGraph-inst.h>
//#include <gtsam/linear/FactorGraphUtil-inl.h>
//#include <gtsam/linear/JacobianFactorGraph.h>
//#include <gtsam/linear/LSPCGSolver.h>
#include <gtsam/linear/PCGSolver.h> #include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h> #include <gtsam/linear/Preconditioner.h>
//#include <gtsam/linear/SuiteSparseUtil.h>
//#include <gtsam/linear/ConjugateGradientMethod-inl.h>
//#include <gsp2/gtsam-interface-sbm.h>
//#include <ydjian/tool/ThreadSafeTimer.h>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <iostream> #include <iostream>
#include <stdexcept> #include <stdexcept>
@ -33,6 +27,7 @@ void PCGSolverParameters::print(ostream &os) const {
/*****************************************************************************/ /*****************************************************************************/
PCGSolver::PCGSolver(const PCGSolverParameters &p) { PCGSolver::PCGSolver(const PCGSolverParameters &p) {
parameters_ = p;
preconditioner_ = createPreconditioner(p.preconditioner_); 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 { void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
/* implement Ax, assume x and Ax are pre-allocated */ /* implement A^T*(A*x), assume x and AtAx are pre-allocated */
/* reset y */ // Build a VectorValues for Vector x
Ax.setZero(); VectorValues vvX = buildVectorValues(x,keyInfo_);
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { // VectorValues form of A'Ax for multiplyHessianAdd
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) { VectorValues vvAtAx;
/* 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<HessianFactor>(gf) ) {
/* accumulate H x */
/* use buffer to avoid excessive table lookups */ // vvAtAx += 1.0 * A'Ax for each factor
const size_t sz = hf->size(); gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
vector<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)));
}
for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) { // Make the result as Vector form
/* retrieve the key mapping */ AtAx = vvAtAx.vector();
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;
}
}
/* 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 { void GaussianFactorGraphSystem::getb(Vector &b) const {
/* compute rhs, assume b pre-allocated */ /* compute rhs, assume b pre-allocated */
/* reset */ // Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues
b.setZero(); VectorValues vvb = gfg_.gradientAtZero();
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { // Make the result as Vector form
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) { b = -vvb.vector();
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<HessianFactor>(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.");
}
}
} }
/**********************************************************************************/ /**********************************************************************************/
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const {
{ preconditioner_.transposeSolve(x, y); } // 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 void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const {
{ preconditioner_.solve(x, y); } // For a preconditioner M = L*L^T
// Calculate y = L^{-T} x
preconditioner_.transposeSolve(x, y);
}
/**********************************************************************************/ /**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, VectorValues buildVectorValues(const Vector &v,

18
gtsam/linear/Preconditioner.cpp
View File

@ -2,7 +2,8 @@
* Preconditioner.cpp * Preconditioner.cpp
* *
* Created on: Jun 2, 2014 * Created on: Jun 2, 2014
* Author: ydjian * Author: Yong-Dian Jian
* Author: Sungtae An
*/ */
#include <gtsam/inference/FactorGraph-inst.h> #include <gtsam/inference/FactorGraph-inst.h>
@ -94,7 +95,7 @@ void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const {
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d); const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
Eigen::Map<Eigen::VectorXd> b(dst, d, 1); Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
R.triangularView<Eigen::Upper>().solveInPlace(b); R.triangularView<Eigen::Lower>().solveInPlace(b);
dst += d; dst += d;
ptr += sz; ptr += sz;
@ -141,11 +142,9 @@ void BlockJacobiPreconditioner::build(
} }
/* getting the block diagonals over the factors */ /* getting the block diagonals over the factors */
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal();
std::map<Key, Matrix> hessianMap = gf->hessianBlockDiagonal(); BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) blocks.push_back(hessian);
blocks.push_back(hessian);
}
/* if necessary, allocating the memory for cacheing the factorization results */ /* if necessary, allocating the memory for cacheing the factorization results */
if ( nnz > bufferSize_ ) { if ( nnz > bufferSize_ ) {
@ -159,11 +158,12 @@ void BlockJacobiPreconditioner::build(
double *ptr = buffer_; double *ptr = buffer_;
for ( size_t i = 0 ; i < n ; ++i ) { for ( size_t i = 0 ; i < n ; ++i ) {
/* use eigen to decompose Di */ /* 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 */ /* store the data in the buffer */
size_t sz = dims_[i]*dims_[i] ; 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 */ /* advance the pointer */
ptr += sz; ptr += sz;

12
gtsam/linear/Preconditioner.h
View File

@ -2,7 +2,8 @@
* Preconditioner.h * Preconditioner.h
* *
* Created on: Jun 2, 2014 * Created on: Jun 2, 2014
* Author: ydjian * Author: Yong-Dian Jian
* Author: Sungtae An
*/ */
#pragma once #pragma once
@ -57,7 +58,8 @@ struct GTSAM_EXPORT PreconditionerParameters {
}; };
/* PCG aims to solve the problem: A x = b by reparametrizing it as /* 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 */ * The goal of this class is to provide a general interface to all preconditioners */
class GTSAM_EXPORT Preconditioner { class GTSAM_EXPORT Preconditioner {
public: public:
@ -70,15 +72,15 @@ public:
/* Computation Interfaces */ /* 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 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 */ /* implement x = L^{-T} y */
virtual void transposeSolve(const Vector& y, Vector& x) const = 0; 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 */ // /* implement x = L^{-1} L^{-T} y */
// virtual void fullSolve(const Vector& y, Vector &x) const = 0; // virtual void fullSolve(const Vector& y, Vector &x) const = 0;
// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; // virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;

154
tests/testPreconditioner.cpp
View File

@ -28,86 +28,98 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
static GaussianFactorGraph createSimpleGaussianFactorGraph() { TEST( PCGsolver, verySimpleLinearSystem) {
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2); // Ax = [4 1][u] = [1] x0 = [2]
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] // [1 3][v] [2] [1]
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); //
// odometry between x1 and x2: x2-x1=[0.2;-0.1] // exact solution x = [1/11, 7/11]';
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); // Create a Gaussian Factor Graph
// measurement between x2 and l1: l1-x2=[-0.2;0.3] GaussianFactorGraph simpleGFG;
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2));
return fg;
// 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<gtsam::PCGSolverParameters>();
pcg->setMaxIterations(500);
pcg->setEpsilon_abs(0.0);
pcg->setEpsilon_rel(0.0);
//pcg->setVerbosity("ERROR");
// With Dummy preconditioner
pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>();
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<gtsam::BlockJacobiPreconditionerParameters>();
// 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 TEST(PCGSolver, simpleLinearSystem) {
std::vector<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) // Create a Gaussian Factor Graph
{ GaussianFactorGraph simpleGFG;
const size_t n = keyInfo.size(); //SharedDiagonal unit2 = noiseModel::Unit::Create(2);
std::vector<size_t> dims_ = keyInfo.colSpec(); 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 */ // Expected solution
std::vector<Matrix> blocks; blocks.reserve(n); 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 */ // Solve the system using direct method
size_t nnz = 0; VectorValues deltaDirect = simpleGFG.optimize();
for ( size_t i = 0 ; i < n ; ++i ) { EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5));
const size_t dim = dims_[i]; //deltaDirect.print("Direct");
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 */ // Solve the system using Preconditioned Conjugate Gradient solver
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { // Common PCG parameters
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) { gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared<gtsam::PCGSolverParameters>();
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { pcg->setMaxIterations(500);
const KeyInfoEntry &entry = keyInfo.find(*it)->second; pcg->setEpsilon_abs(0.0);
const Matrix &Ai = jf->getA(it); pcg->setEpsilon_rel(0.0);
blocks[entry.index()] += (Ai.transpose() * Ai); //pcg->setVerbosity("ERROR");
}
}
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.");
}
}
return blocks; // With Dummy preconditioner
} pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>();
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5));
//deltaPCGDummy.print("PCG Dummy");
/* ************************************************************************* */ // With Block-Jacobi preconditioner
TEST( Preconditioner, buildBlocks ) { pcg->preconditioner_ = boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
// Create simple Gaussian factor graph and initial values VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
Values initial; //deltaPCGJacobi.print("PCG Jacobi");
initial.insert(0,Point2(4, 5));
initial.insert(1,Point2(0, 1));
initial.insert(2,Point2(-5, 7));
// Expected Hessian block diagonal matrices
std::map<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
// Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
std::vector<Matrix> 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<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
std::vector<Matrix>::const_iterator it2 = actualHessian.begin();
for(; it1!=expectedHessian.end(); it1++, it2++)
EXPECT(assert_equal(it1->second, *it2));
} }
/* ************************************************************************* */ /* ************************************************************************* */