diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 40d49a934..46b243913 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -42,6 +42,21 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { return fg; } +/* ************************************************************************* */ +static GaussianFactorGraph createSimpleGaussianFactorGraphUnordered() { + 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), 1, 10*eye(2), (Vector(2) << 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), (Vector(2) << 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), (Vector(2) << -1.0, 1.5), unit2); + return fg; +} + /* ************************************************************************* */ // Copy of BlockJacobiPreconditioner::build std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) @@ -89,10 +104,6 @@ std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo & 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)); // Expected Hessian block diagonal matrices std::map expectedHessian =gfg.hessianBlockDiagonal(); @@ -110,6 +121,28 @@ TEST( Preconditioner, buildBlocks ) { EXPECT(assert_equal(it1->second, *it2)); } +/* ************************************************************************* */ +TEST( Preconditioner, buildBlocks2 ) { + // Create simple Gaussian factor graph and initial values + GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered(); + + // Expected Hessian block diagonal matrices + std::map expectedHessian =gfg.hessianBlockDiagonal(); + + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector actualHessian = buildBlocks(gfg, KeyInfo(gfg)); + + // Compare the number of block diagonal matrices + EXPECT_LONGS_EQUAL(expectedHessian.size(), 3); + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + + // Compare the values of matrices + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + for(; it1!=expectedHessian.end(); it1++, it2++) + EXPECT(assert_equal(it1->second, *it2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */