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

Add a unit test for GaussianFactorGraphSystem::multiply and getb

release/4.3a0
Sungtae An 2015-01-31 03:33:19 -05:00
parent 4bc9ee6787
commit e1ace9b62a
1 changed files with 43 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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( PCGSolver, dummy )