diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index db17f1844..27eb57b44 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -33,7 +33,6 @@ void PCGSolverParameters::print(ostream &os) const { /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { - parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 7683a0efd..c7f4a5b68 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -143,9 +143,11 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - std::map hessianMap =gfg.hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + std::map hessianMap = gf->hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); + } /* if necessary, allocating the memory for cacheing the factorization results */ if ( nnz > bufferSize_ ) { diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index efcb28710..10ceb78a9 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -151,14 +151,6 @@ public: const std::map &lambda ) ; - // Should be removed after test - double* getBuffer() { - return buffer_; - } - size_t getBufferSize() { - return bufferSize_; - } - protected: void clean() ; diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index bcb59afe7..46b243913 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -100,7 +100,7 @@ std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo & return blocks; } -/* ************************************************************************* +/* ************************************************************************* */ TEST( Preconditioner, buildBlocks ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); @@ -121,7 +121,7 @@ TEST( Preconditioner, buildBlocks ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* +/* ************************************************************************* */ TEST( Preconditioner, buildBlocks2 ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered(); @@ -143,221 +143,6 @@ TEST( Preconditioner, buildBlocks2 ) { 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), (Vector(2) << 1,2 ), 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] - // - Matrix expectedH0 = it1->second; - Matrix actualH0 = *it2; - EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10) )); - EXPECT(assert_equal(expectedH0, actualH0)); - - // The corresponding Cholesky decomposition is: - // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - Vector expectedR = (Vector(4) << 4.1231, 0, 1.6977, 2.6679); - double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){ - EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); - } - -} - -/* ************************************************************************* */ -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), (Vector(2) << -1, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), 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 - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - double* buf = blockJacobi->getBuffer(); - for(size_t i=0; igetBufferSize(); ++i){ - std::cout << "buf[" << i << "] = " << buf[i] << std::endl; - } - -} - -/* ************************************************************************* */ -TEST( PCGsolver, verySimpleLinearSystem) { - - // 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), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); - //simpleGFG.print("Factors\n"); - - // Exact solution already known - VectorValues exactSolution; - exactSolution.insert(0, (Vector(2) << 1./11., 7./11.)); - //exactSolution.print("Exact"); - - // Solve the system using direct method - VectorValues deltaDirect = simpleGFG.optimize(); - EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); - //deltaDirect.print("Direct"); - - // Solve the system using PCG - // With Dummy preconditioner - gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); - pcg->setMaxIterations(500); - pcg->setEpsilon_abs(0.0); - pcg->setEpsilon_rel(0.0); - //pcg->setVerbosity("ERROR"); - VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); - EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); - - // With Block-Jacobi preconditioner - gtsam::PCGSolverParameters::shared_ptr pcgJacobi = boost::make_shared(); - pcgJacobi->preconditioner_ = boost::make_shared(); - pcgJacobi->setMaxIterations(500); - pcgJacobi->setEpsilon_abs(0.0); - pcgJacobi->setEpsilon_rel(0.0); - VectorValues deltaPCGJacobi = PCGSolver(*pcgJacobi).optimize(simpleGFG); - - // Failed! - EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); - //deltaPCGJacobi.print("PCG Jacobi"); -} - -/* ************************************************************************* */ -TEST(PCGSolver, simpleLinearSystem) { - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - //simpleGFG.print("Factors\n"); - - // Expected solution - VectorValues expectedSolution; - expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756)); - expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577)); - expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582)); - - // Solve the system using direct method - VectorValues deltaDirect = simpleGFG.optimize(); - EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); - //expectedSolution.print("Expected"); - //deltaCholesky.print("Direct"); - - // Solve the system using PCG - VectorValues initial; - initial.insert(0, (Vector(2) << 0.1, -0.1)); - initial.insert(1, (Vector(2) << -0.1, 0.1)); - initial.insert(2, (Vector(2) << -0.1, -0.1)); - - // With Dummy preconditioner - gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); - pcg->setMaxIterations(500); - pcg->setEpsilon_abs(0.0); - pcg->setEpsilon_rel(0.0); - //pcg->setVerbosity("ERROR"); - - VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); - // Failed! - EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); - //deltaPCGDummy.print("PCG Dummy"); - - // Solve the system using Preconditioned Conjugate Gradient - pcg->preconditioner_ = boost::make_shared(); - VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); - // Failed! - EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); - //deltaPCGJacobi.print("PCG Jacobi"); - - // Test that the retrieval of the diagonal blocks of the Jacobian are correct. - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - // The corresponding Cholesky decomposition is: - // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){} - // TODO: EXPECT(assert_equal(number..,buf[i])); - - size_t i = 0; - for(; it1!=expectedHessian.end(); it1++, it2++){ - EXPECT(assert_equal(it1->second, *it2)); - Matrix R_i(2,2); - R_i(0,0) = buf[i+0]; - R_i(0,1) = buf[i+1]; - R_i(1,0) = buf[i+2]; - R_i(1,1) = buf[i+3]; - - Matrix actualH_i = R_i.transpose() * R_i;// i-th diagonal block - EXPECT(assert_equal(it1->second, actualH_i)); - i += 4; - } -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */