diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 98f2fba4e..4764a1b21 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -27,199 +27,6 @@ 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), (Vector(2) << 2.0, -1.0).finished(), 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).finished(), 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).finished(), unit2); - 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).finished(), 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).finished(), 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).finished(), unit2); - return fg; -} - -/* ************************************************************************* */ -// Copy of BlockJacobiPreconditioner::build -std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) -{ - const size_t n = keyInfo.size(); - std::vector dims_ = keyInfo.colSpec(); - - /* prepare the buffer of block diagonals */ - std::vector blocks; blocks.reserve(n); - - /* 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; - } - - /* 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(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(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."); - } - } - - return blocks; -} - -/* ************************************************************************* */ -TEST( Preconditioner, buildBlocks ) { - // Create simple Gaussian factor graph and initial values - GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - - // 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(), actualHessian.size()); - - // Compare the values of matrices - // This test should be failed when the noise model is not isotropic. - 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)); -} - -/* ************************************************************************* */ -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 - // This test should be failed when the noise model is not isotropic. - 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)); -} - -/* ************************************************************************* */ -TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { - // 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)); - //simpleGFG.print("Factors\n"); - - // Expected Hessian block diagonal matrices - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - // Compare the number of block diagonal matrices - 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(); - - // the function 'build' in BlockJacobianPreconditioner stores in 'buffer' - // the cholesky decomposion of each block of the hessian (column major) - // In this example there is a single block (i.e., a single value) - // and the corresponding block of the Hessian is - // - // H0 = [17 7; 7 10] - // - // The corresponding Cholesky decomposition is: - // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) - - Matrix expectedH0 = it1->second; - Matrix actualH0 = *it2; - - // This test should be failed when the noise model is not isotropic. - EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10).finished() )); - EXPECT(assert_equal(expectedH0, actualH0)); - -} - -/* ************************************************************************* */ -TEST( BlockJacobiPreconditioner, SimpleLinerSystem) { - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - 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); - - // Expected Hessian block diagonal matrices - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - // Compare the number of block diagonal matrices - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - - // Compare the values of matrices - // This test should be failed when the noise model is not isotropic. - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - for(; it1!=expectedHessian.end(); it1++, it2++){ - Matrix expectedHi = it1->second; - Matrix actualHi = *it2; - EXPECT(assert_equal(expectedHi, actualHi)); - } -} - /* ************************************************************************* */ TEST( PCGsolver, verySimpleLinearSystem) {