Sungtae An
commit
c1f048dc42
|
|
@ -2,7 +2,7 @@
|
|||
* ConjugateGradientSolver.cpp
|
||||
*
|
||||
* Created on: Jun 4, 2014
|
||||
* Author: ydjian
|
||||
* Author: Yong-Dian Jian
|
||||
*/
|
||||
|
||||
#include <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 <gtsam/linear/IterativeSolver.h>
|
||||
|
|
@ -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 <class S, class V>
|
||||
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
|
||||
|
|
|
|||
|
|
@ -2,20 +2,14 @@
|
|||
* PCGSolver.cpp
|
||||
*
|
||||
* Created on: Feb 14, 2012
|
||||
* Author: ydjian
|
||||
* Author: Yong-Dian Jian
|
||||
* Author: Sungtae An
|
||||
*/
|
||||
|
||||
#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/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 <iostream>
|
||||
#include <stdexcept>
|
||||
|
|
@ -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<JacobianFactor>(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<HessianFactor>(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<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<JacobianFactor>(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<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.");
|
||||
}
|
||||
}
|
||||
// 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,
|
||||
|
|
|
|||
|
|
@ -2,7 +2,8 @@
|
|||
* Preconditioner.cpp
|
||||
*
|
||||
* Created on: Jun 2, 2014
|
||||
* Author: ydjian
|
||||
* Author: Yong-Dian Jian
|
||||
* Author: Sungtae An
|
||||
*/
|
||||
|
||||
#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);
|
||||
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
|
||||
R.triangularView<Eigen::Upper>().solveInPlace(b);
|
||||
R.triangularView<Eigen::Lower>().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<Key, Matrix> hessianMap = gf->hessianBlockDiagonal();
|
||||
std::map<Key, Matrix> 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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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<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
|
||||
std::vector<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo)
|
||||
{
|
||||
const size_t n = keyInfo.size();
|
||||
std::vector<size_t> 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<Matrix> 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<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.");
|
||||
}
|
||||
}
|
||||
// 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");
|
||||
|
||||
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");
|
||||
|
||||
/* ************************************************************************* */
|
||||
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<gtsam::BlockJacobiPreconditionerParameters>();
|
||||
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
|
||||
//deltaPCGJacobi.print("PCG Jacobi");
|
||||
|
||||
// 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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue