From e1ace9b62aa4641211beb7c68f995de11fc28a35 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Sat, 31 Jan 2015 03:33:19 -0500 Subject: [PATCH] Add a unit test for GaussianFactorGraphSystem::multiply and getb --- tests/testPCGSolver.cpp | 43 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index da3462b57..c90b09db1 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -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 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 )