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

Merged in feature/fixPCGSolver (pull request #94) 

Resolve PCG solver test failure on Mac
release/4.3a0
Frank Dellaert 2015-02-10 16:55:38 +01:00
parent a72e450fbf e1ace9b62a
commit 6134c01e19
2 changed files with 45 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/linear/PCGSolver.cpp
View File

@ -84,8 +84,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
// Make the result as Vector form // Make the result as Vector form
AtAx = vvAtAx.vector(); AtAx = vvAtAx.vector(keyInfo_.ordering());
} }
/*****************************************************************************/ /*****************************************************************************/
@ -96,7 +95,7 @@ void GaussianFactorGraphSystem::getb(Vector &b) const {
VectorValues vvb = gfg_.gradientAtZero(); VectorValues vvb = gfg_.gradientAtZero();
// Make the result as Vector form // Make the result as Vector form
b = -vvb.vector(); b = -vvb.vector(keyInfo_.ordering());
} }
/**********************************************************************************/ /**********************************************************************************/

43
tests/testPCGSolver.cpp
View File

@ -91,6 +91,49 @@ TEST( PCGSolver, llt ) {
} }
/* ************************************************************************* */
// Test GaussianFactorGraphSystem::multiply and getb
TEST( GaussianFactorGraphSystem, multiply_getb)
{
// Create a Gaussian Factor Graph
GaussianFactorGraph simpleGFG;
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);
// Create a dummy-preconditioner and a GaussianFactorGraphSystem
DummyPreconditioner dummyPreconditioner;
KeyInfo keyInfo(simpleGFG);
std::map<Key,Vector> lambda;
dummyPreconditioner.build(simpleGFG, keyInfo, lambda);
GaussianFactorGraphSystem gfgs(simpleGFG, dummyPreconditioner, keyInfo, lambda);
// Prepare container for each variable
Vector initial, residual, preconditionedResidual, p, actualAp;
initial = (Vector(6) << 0., 0., 0., 0., 0., 0.).finished();
// Calculate values using GaussianFactorGraphSystem same as inside of PCGSolver
gfgs.residual(initial, residual); /* r = b-Ax */
gfgs.leftPrecondition(residual, preconditionedResidual); /* pr = L^{-1} (b-Ax) */
gfgs.rightPrecondition(preconditionedResidual, p); /* p = L^{-T} pr */
gfgs.multiply(p, actualAp); /* A p */
// Expected value of Ap for the first iteration of this example problem
Vector expectedAp = (Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished();
EXPECT(assert_equal(expectedAp, actualAp, 1e-3));
// Expected value of getb
Vector expectedb = (Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished();
Vector actualb;
gfgs.getb(actualb);
EXPECT(assert_equal(expectedb, actualb, 1e-3));
}
/* ************************************************************************* */ /* ************************************************************************* */
// Test Dummy Preconditioner // Test Dummy Preconditioner
TEST( PCGSolver, dummy ) TEST( PCGSolver, dummy )