From cf34726a81d6a2ade1a7b1b145e439afa324a1ea Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 1 Dec 2014 04:57:06 -0500 Subject: [PATCH 01/24] Fix PCG solver parameter initialization --- gtsam/linear/PCGSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 27eb57b44..db17f1844 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -33,6 +33,7 @@ void PCGSolverParameters::print(ostream &os) const { /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { + parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } From bc6ce27b28647b83eef12ae42fe1e25876cf8078 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 1 Dec 2014 04:58:05 -0500 Subject: [PATCH 02/24] Fix build function in Preconditioner --- gtsam/linear/Preconditioner.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 9af362fba..59f912ad9 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -141,11 +141,9 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - 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); - } + std::map hessianMap =gfg.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_ ) { From 6c3df407a1d3a027f8d8a29c98f4f09128f7462e Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 1 Dec 2014 04:58:55 -0500 Subject: [PATCH 03/24] Add temporary getBuffer function and it should be removed later --- gtsam/linear/Preconditioner.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 10ceb78a9..61df4414d 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -151,6 +151,10 @@ public: const std::map &lambda ) ; + // Should be removed after test + double* getBuffer() { + return buffer_; + } protected: void clean() ; From 332b3f9da967ab39cc36771bcd6893adac6bab4c Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 1 Dec 2014 05:02:02 -0500 Subject: [PATCH 04/24] Add tests for preconditioner and solver --- tests/testPreconditioner.cpp | 223 +++++++++++++++++++++++++++++++++-- 1 file changed, 216 insertions(+), 7 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index ad2c0767a..4b781201f 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -34,11 +34,26 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { // 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), Vector2(2.0, -1.0), unit2); + fg += JacobianFactor(2, -10*eye(2), 0, 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), Vector2(0.0, 1.0), unit2); + 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), Vector2(-1.0, 1.5), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), 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), 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; } @@ -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,204 @@ 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)); +} + +/* ************************************************************************* */ +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 + // 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] + // + EXPECT(assert_equal(it1->second, *it2)); + // TODO: Matrix expectedH0 ... + //EXPECT(assert_equal(it1->second, *it2)); + + // 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])); + +} +/* ************************************************************************* */ +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); } /* ************************************************************************* */ From e467f56b74690456ac8bdcae211baf054803693a Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 1 Dec 2014 05:02:23 -0500 Subject: [PATCH 05/24] Add temporary function of getBufferSize which will be removed later --- gtsam/linear/Preconditioner.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 61df4414d..efcb28710 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -155,6 +155,10 @@ public: double* getBuffer() { return buffer_; } + size_t getBufferSize() { + return bufferSize_; + } + protected: void clean() ; From b601eb0f9299967a4452fb73045d9243a05e402c Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 1 Dec 2014 05:04:17 -0500 Subject: [PATCH 06/24] Add temporary tests --- tests/testPreconditioner.cpp | 57 ++++++++++++++++++++++++++++++------ 1 file changed, 48 insertions(+), 9 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 4b781201f..bcb59afe7 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,7 +143,7 @@ TEST( Preconditioner, buildBlocks2 ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* */ +/* ************************************************************************* TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { // Ax = [4 1][u] = [1] x0 = [2] // [1 3][v] [2] [1] @@ -168,26 +168,65 @@ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { std::vector::const_iterator it2 = actualHessian.begin(); // the function 'build' in BlockJacobianPreconditioner stores in 'buffer' - // the cholesky decomposion of each block of the hessian + // 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] // - EXPECT(assert_equal(it1->second, *it2)); - // TODO: Matrix expectedH0 ... - //EXPECT(assert_equal(it1->second, *it2)); + 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){} - // TODO: EXPECT(assert_equal(number..,buf[i])); + 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) { From f3bbe604d607dcb81d6ad4e80d3bc2ce06e44301 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 1 Dec 2014 05:07:43 -0500 Subject: [PATCH 07/24] temporary printing out for test Fix Eigen comma initialization --- tests/testPreconditioner.cpp | 62 ++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index bcb59afe7..7175059b9 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -34,11 +34,11 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { // 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), unit2); + 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), unit2); + 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), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5).finished(), unit2); return fg; } @@ -49,11 +49,11 @@ static GaussianFactorGraph createSimpleGaussianFactorGraphUnordered() { // 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); + 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), unit2); + 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), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5).finished(), unit2); return fg; } @@ -153,7 +153,7 @@ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { // 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 += 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 @@ -176,7 +176,7 @@ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { // Matrix expectedH0 = it1->second; Matrix actualH0 = *it2; - EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10) )); + EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10).finished() )); EXPECT(assert_equal(expectedH0, actualH0)); // The corresponding Cholesky decomposition is: @@ -184,7 +184,7 @@ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { 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); + Vector expectedR = (Vector(4) << 4.1231, 0, 1.6977, 2.6679).finished(); double* buf = blockJacobi->getBuffer(); for(int i=0; i<4; ++i){ EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); @@ -197,13 +197,13 @@ 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); + 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(); @@ -238,12 +238,12 @@ TEST( PCGsolver, verySimpleLinearSystem) { // 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 += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2)); //simpleGFG.print("Factors\n"); // Exact solution already known VectorValues exactSolution; - exactSolution.insert(0, (Vector(2) << 1./11., 7./11.)); + exactSolution.insert(0, (Vector(2) << 1./11., 7./11.).finished()); //exactSolution.print("Exact"); // Solve the system using direct method @@ -280,20 +280,20 @@ 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 += 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); //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)); + expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished()); + expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished()); + expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished()); // Solve the system using direct method VectorValues deltaDirect = simpleGFG.optimize(); @@ -303,9 +303,9 @@ TEST(PCGSolver, simpleLinearSystem) { // 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)); + initial.insert(0, (Vector(2) << 0.1, -0.1).finished()); + initial.insert(1, (Vector(2) << -0.1, 0.1).finished()); + initial.insert(2, (Vector(2) << -0.1, -0.1).finished()); // With Dummy preconditioner gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); From 49bca1ac6994d1889269b7dd18746f96d4d1bc41 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 1 Dec 2014 05:11:26 -0500 Subject: [PATCH 08/24] Add file info --- gtsam/linear/ConjugateGradientSolver.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 6e8509309..4d4623d05 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -9,6 +9,14 @@ * -------------------------------------------------------------------------- */ +/** + * @file ConjugateGradientSolver.h + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Sungtae An + * @author ydjian + * @date Nov 6, 2014 + **/ + #pragma once #include From c4841c6e219b718e3fda96748bb73c7edd47c5a8 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 3 Dec 2014 12:17:52 -0500 Subject: [PATCH 09/24] Add a comment --- gtsam/linear/Preconditioner.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 59f912ad9..576274de2 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -157,6 +157,7 @@ void BlockJacobiPreconditioner::build( double *ptr = buffer_; for ( size_t i = 0 ; i < n ; ++i ) { /* use eigen to decompose Di */ + /* It is same as R = chol(M) in MATLAB where M is full preconditioner */ const Matrix R = blocks[i].llt().matrixL().transpose(); /* store the data in the buffer */ From 5029d84ce25d61033b431b964b292390cdce2e6a Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 3 Dec 2014 12:19:01 -0500 Subject: [PATCH 10/24] Fix function 'multiply' --- gtsam/linear/PCGSolver.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index db17f1844..ab3e9ba98 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -77,26 +77,31 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { } /*****************************************************************************/ -void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const { - /* implement Ax, assume x and Ax are pre-allocated */ +void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { + /* implement A^t*A*x, assume x and AtAx are pre-allocated */ /* reset y */ - Ax.setZero(); + AtAx.setZero(); BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { /* accumulate At A x*/ + Vector Ax = zero(jf->rows()); for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { const Matrix Ai = jf->getA(it); /* this map lookup should be replaced */ const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - Ax.segment(entry.colstart(), entry.dim()) - += Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim())); + Ax += (Ai * x.segment(entry.colstart(), entry.dim())); + } + for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { + const Matrix Ai = jf->getA(it); + /* this map lookup should be replaced */ + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + AtAx.segment(entry.colstart(), entry.dim()) += Ai.transpose()*Ax; } } else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { /* accumulate H x */ - /* use buffer to avoid excessive table lookups */ const size_t sz = hf->size(); vector y; @@ -122,7 +127,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const { for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) { /* retrieve the key mapping */ const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second; - Ax.segment(entry.colstart(), entry.dim()) += y[i]; + AtAx.segment(entry.colstart(), entry.dim()) += y[i]; } } else { From 08f4c82edcdeb2b4271836616bb6bda9445b7967 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 3 Dec 2014 12:21:35 -0500 Subject: [PATCH 11/24] Replace the codes in the function 'multiply' with 'multiplyHessianAdd' --- gtsam/linear/PCGSolver.cpp | 57 +++----------------------------------- 1 file changed, 4 insertions(+), 53 deletions(-) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index ab3e9ba98..fc226eb3e 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -80,60 +80,11 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { /* implement A^t*A*x, assume x and AtAx are pre-allocated */ - /* reset y */ - AtAx.setZero(); + VectorValues vvX = buildVectorValues(x,keyInfo_); + VectorValues vvAtAx; + gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); + AtAx = vvAtAx.vector(); - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate At A x*/ - Vector Ax = zero(jf->rows()); - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - const Matrix Ai = jf->getA(it); - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - Ax += (Ai * x.segment(entry.colstart(), entry.dim())); - } - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - const Matrix Ai = jf->getA(it); - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - AtAx.segment(entry.colstart(), entry.dim()) += Ai.transpose()*Ax; - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate H x */ - /* use buffer to avoid excessive table lookups */ - const size_t sz = hf->size(); - vector y; - y.reserve(sz); - for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { - /* initialize y to zeros */ - y.push_back(zero(hf->getDim(it))); - } - - for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) { - /* retrieve the key mapping */ - const KeyInfoEntry &entry = keyInfo_.find(*j)->second; - // xj is the input vector - const Vector xj = x.segment(entry.colstart(), entry.dim()); - size_t idx = 0; - for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) { - if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj; - else y[idx] += hf->info(i, j).knownOffDiagonal() * xj; - } - } - - /* accumulate to r */ - for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) { - /* retrieve the key mapping */ - const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second; - AtAx.segment(entry.colstart(), entry.dim()) += y[i]; - } - } - else { - throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } } /*****************************************************************************/ From f9d6c3da227556d09cfcc3f8fb2ef35a399ed22d Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 3 Dec 2014 12:30:56 -0500 Subject: [PATCH 12/24] Interchange solve and transposeSolve for leftPrecondition and rightPrecondition --- gtsam/linear/PCGSolver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index fc226eb3e..0ce2b1e52 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -119,11 +119,11 @@ void GaussianFactorGraphSystem::getb(Vector &b) const { /**********************************************************************************/ void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.transposeSolve(x, y); } +{ preconditioner_.solve(x, y); } /**********************************************************************************/ void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.solve(x, y); } +{ preconditioner_.transposeSolve(x, y); } /**********************************************************************************/ VectorValues buildVectorValues(const Vector &v, From 60f43c7a4b135f8b5c706a6f83384af9604d81c2 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 4 Dec 2014 22:08:26 -0500 Subject: [PATCH 13/24] Fix a bug in getb and replace it with negated values of gradientAtZero. Add some comments about a bug. --- gtsam/linear/PCGSolver.cpp | 67 +++++++++++++++++++++++++------------- 1 file changed, 44 insertions(+), 23 deletions(-) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 0ce2b1e52..3e57cdf58 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -3,6 +3,7 @@ * * Created on: Feb 14, 2012 * Author: ydjian + * Author: Sungtae An */ #include @@ -80,9 +81,13 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { /* implement A^t*A*x, assume x and AtAx are pre-allocated */ + // Build a VectorValues for Vector x VectorValues vvX = buildVectorValues(x,keyInfo_); + // VectorValues form of A'Ax for multiplyHessianAdd VectorValues vvAtAx; + // vvAtAx += 1.0 * A'Ax for each factor gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); + // Make the result as Vector form AtAx = vvAtAx.vector(); } @@ -91,30 +96,46 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { void GaussianFactorGraphSystem::getb(Vector &b) const { /* compute rhs, assume b pre-allocated */ - /* reset */ - b.setZero(); + /* ------------------------------------------------------------------------ + * Multiply and getb functions (build function in preconditioner.cpp also) + * Yong-Dian's code had a bug that they do not consider noise model + * which means that they do not whiten A and b. + * It has no problem when the associated noise model has a form of Isotropic + * because it can be cancelled out on both l.h.s and r.h.s of equation. + * However, it cause a wrong result with non-isotropic noise model. + * The unit test for PCSSolver (testPCGSolver.cpp) Yond-Dian made use a + * example factor graph which has isotropic noise model and + * that is the reason why there was no unit test error. + * ------------------------------------------------------------------------*/ +// /* reset */ +// b.setZero(); +// +// BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { +// if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { +// const Vector rhs = jf->getb(); +// /* accumulate At rhs */ +// for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { +// /* this map lookup should be replaced */ +// const KeyInfoEntry &entry = keyInfo_.find(*it)->second; +// b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ; +// } +// } +// else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { +// /* accumulate g */ +// for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { +// const KeyInfoEntry &entry = keyInfo_.find(*it)->second; +// b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it); +// } +// } +// else { +// throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); +// } +// } - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - const Vector rhs = jf->getb(); - /* accumulate At rhs */ - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ; - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate g */ - for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it); - } - } - else { - throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } + // Get whitened r.h.s (b vector) from each factor in the form of VectorValues + VectorValues vvb = gfg_.gradientAtZero(); + // Make the result as Vector form + b = -vvb.vector(); } /**********************************************************************************/ From c2b5b152a48002252c7a30eccd358f685d986e5c Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 4 Dec 2014 22:14:46 -0500 Subject: [PATCH 14/24] Change the noise model as non-isotropic in the test of PCG solver with a simple linear system --- tests/testPreconditioner.cpp | 80 ++++++++++++++++++------------------ 1 file changed, 41 insertions(+), 39 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 7175059b9..59c493c85 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,7 +143,7 @@ TEST( Preconditioner, buildBlocks2 ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* +/* ************************************************************************* */ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { // Ax = [4 1][u] = [1] x0 = [2] // [1 3][v] [2] [1] @@ -187,7 +187,7 @@ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { Vector expectedR = (Vector(4) << 4.1231, 0, 1.6977, 2.6679).finished(); double* buf = blockJacobi->getBuffer(); for(int i=0; i<4; ++i){ - EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); + EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); } } @@ -222,7 +222,7 @@ TEST( BlockJacobiPreconditioner, SimpleLinerSystem) { 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; + std::cout << "buf[" << i << "] = " << buf[i] << std::endl; } } @@ -244,12 +244,12 @@ TEST( PCGsolver, verySimpleLinearSystem) { // Exact solution already known VectorValues exactSolution; exactSolution.insert(0, (Vector(2) << 1./11., 7./11.).finished()); - //exactSolution.print("Exact"); + exactSolution.print("Exact"); // Solve the system using direct method VectorValues deltaDirect = simpleGFG.optimize(); EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); - //deltaDirect.print("Direct"); + deltaDirect.print("Direct"); // Solve the system using PCG // With Dummy preconditioner @@ -261,25 +261,27 @@ TEST( PCGsolver, verySimpleLinearSystem) { //pcg->setVerbosity("ERROR"); VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); + deltaPCGDummy.print("PCG Dummy"); // With Block-Jacobi preconditioner gtsam::PCGSolverParameters::shared_ptr pcgJacobi = boost::make_shared(); pcgJacobi->preconditioner_ = boost::make_shared(); - pcgJacobi->setMaxIterations(500); + pcgJacobi->setMaxIterations(1500);// It takes more than 1000 iterations for this test 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"); + deltaPCGJacobi.print("PCG Jacobi"); } /* ************************************************************************* */ TEST(PCGSolver, simpleLinearSystem) { // Create a Gaussian Factor Graph GaussianFactorGraph simpleGFG; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); + //SharedDiagonal unit2 = noiseModel::Unit::Create(2); + 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); @@ -298,8 +300,8 @@ TEST(PCGSolver, simpleLinearSystem) { // Solve the system using direct method VectorValues deltaDirect = simpleGFG.optimize(); EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); - //expectedSolution.print("Expected"); - //deltaCholesky.print("Direct"); + expectedSolution.print("Expected"); + deltaDirect.print("Direct"); // Solve the system using PCG VectorValues initial; @@ -318,44 +320,44 @@ TEST(PCGSolver, simpleLinearSystem) { VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); // Failed! EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); - //deltaPCGDummy.print("PCG Dummy"); + 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"); + 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(); + 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])); + // 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]; + 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; - } + Matrix actualH_i = R_i.transpose() * R_i;// i-th diagonal block + EXPECT(assert_equal(it1->second, actualH_i)); + i += 4; + } } /* ************************************************************************* */ From 4777c029ac159fcfb98dc76659c777a3ed0149b7 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 4 Dec 2014 22:32:07 -0500 Subject: [PATCH 15/24] Remove temporary testable functions (getBuffer and getBufferSize) --- gtsam/linear/Preconditioner.h | 8 -------- 1 file changed, 8 deletions(-) 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() ; From 7760a802ab7f8068772aa8ca7fc864f1572ff544 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 4 Dec 2014 22:33:53 -0500 Subject: [PATCH 16/24] Remove tests using deprecated testable functions (getBuffer, getBufferSize) --- tests/testPreconditioner.cpp | 64 ++++++++---------------------------- 1 file changed, 14 insertions(+), 50 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 59c493c85..98f2fba4e 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -115,6 +115,7 @@ TEST( Preconditioner, buildBlocks ) { 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++) @@ -137,6 +138,7 @@ TEST( Preconditioner, buildBlocks2 ) { 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++) @@ -174,21 +176,15 @@ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { // // H0 = [17 7; 7 10] // - Matrix expectedH0 = it1->second; - Matrix actualH0 = *it2; - EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10).finished() )); - 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).finished(); - double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){ - EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); - } + + 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)); } @@ -213,18 +209,15 @@ TEST( BlockJacobiPreconditioner, SimpleLinerSystem) { 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(); - - 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; + for(; it1!=expectedHessian.end(); it1++, it2++){ + Matrix expectedHi = it1->second; + Matrix actualHi = *it2; + EXPECT(assert_equal(expectedHi, actualHi)); } - } /* ************************************************************************* */ @@ -329,35 +322,6 @@ TEST(PCGSolver, simpleLinearSystem) { 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; - } } /* ************************************************************************* */ From 558bee685ee1d93f791c3b0fc0bb5aa0e383abdf Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 4 Dec 2014 22:57:44 -0500 Subject: [PATCH 17/24] Remove tests associated with older version (Yong-Dian) of BlockJacobiPreconditioner::build --- tests/testPreconditioner.cpp | 193 ----------------------------------- 1 file changed, 193 deletions(-) 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) { From 55b8ecf8fac9e864e2b6bc7642345a906402083f Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 4 Dec 2014 23:29:12 -0500 Subject: [PATCH 18/24] Removed the commented old version of Yong-Dian's code for getb --- gtsam/linear/PCGSolver.cpp | 54 ++++++++++---------------------------- 1 file changed, 14 insertions(+), 40 deletions(-) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 3e57cdf58..23f4485f9 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -83,10 +83,13 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { // Build a VectorValues for Vector x VectorValues vvX = buildVectorValues(x,keyInfo_); + // VectorValues form of A'Ax for multiplyHessianAdd VectorValues vvAtAx; + // vvAtAx += 1.0 * A'Ax for each factor gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); + // Make the result as Vector form AtAx = vvAtAx.vector(); @@ -96,55 +99,26 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { void GaussianFactorGraphSystem::getb(Vector &b) const { /* compute rhs, assume b pre-allocated */ - /* ------------------------------------------------------------------------ - * Multiply and getb functions (build function in preconditioner.cpp also) - * Yong-Dian's code had a bug that they do not consider noise model - * which means that they do not whiten A and b. - * It has no problem when the associated noise model has a form of Isotropic - * because it can be cancelled out on both l.h.s and r.h.s of equation. - * However, it cause a wrong result with non-isotropic noise model. - * The unit test for PCSSolver (testPCGSolver.cpp) Yond-Dian made use a - * example factor graph which has isotropic noise model and - * that is the reason why there was no unit test error. - * ------------------------------------------------------------------------*/ -// /* reset */ -// b.setZero(); -// -// BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { -// if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { -// const Vector rhs = jf->getb(); -// /* accumulate At rhs */ -// for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { -// /* this map lookup should be replaced */ -// const KeyInfoEntry &entry = keyInfo_.find(*it)->second; -// b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ; -// } -// } -// else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { -// /* accumulate g */ -// for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { -// const KeyInfoEntry &entry = keyInfo_.find(*it)->second; -// b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it); -// } -// } -// else { -// throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); -// } -// } - // Get whitened r.h.s (b vector) from each factor in the form of VectorValues VectorValues vvb = gfg_.gradientAtZero(); + // Make the result as Vector form b = -vvb.vector(); } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.solve(x, y); } +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { + // For a preconditioner M = L*L^T + // Calculate y = L^{-1} x + preconditioner_.solve(x, y); +} /**********************************************************************************/ -void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.transposeSolve(x, y); } +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const { + // For a preconditioner M = L*L^T + // Calculate y = L^{-T} x + preconditioner_.transposeSolve(x, y); +} /**********************************************************************************/ VectorValues buildVectorValues(const Vector &v, From cfb82d9a960b607cb61bdb55fd23303d84a10592 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 4 Dec 2014 23:30:17 -0500 Subject: [PATCH 19/24] Modify some comments --- gtsam/linear/ConjugateGradientSolver.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 4d4623d05..ddb6e26bb 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -90,8 +90,8 @@ public: /*********************************************************************************************/ /* * A template of linear preconditioned conjugate gradient method. - * System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v, - * rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. */ template @@ -124,19 +124,19 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju if ( k % iReset == 0 ) { system.residual(estimate, q1); /* q1 = b-Ax */ - system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ - system.rightPrecondition(residual, direction); /* d = S^{-1} r */ + system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ + system.rightPrecondition(residual, direction); /* d = L^{-T} r */ currentGamma = system.dot(residual, residual); } system.multiply(direction, q1); /* q1 = A d */ alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */ system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */ - system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */ + system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */ system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */ prevGamma = currentGamma; currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */ beta = currentGamma / prevGamma; - system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */ + system.rightPrecondition(residual, q1); /* q1 = L^{-T} residual */ system.scal(beta, direction); system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */ From 06b82ce3e324fb549d83c03b242c3d6f0fe38e86 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 4 Dec 2014 23:30:27 -0500 Subject: [PATCH 20/24] Clean up the code --- tests/testPreconditioner.cpp | 57 +++++++++++++++--------------------- 1 file changed, 24 insertions(+), 33 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 4764a1b21..43b4935fc 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -39,41 +39,39 @@ TEST( PCGsolver, verySimpleLinearSystem) { // 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"); // Exact solution already known VectorValues exactSolution; exactSolution.insert(0, (Vector(2) << 1./11., 7./11.).finished()); - exactSolution.print("Exact"); + //exactSolution.print("Exact"); // Solve the system using direct method VectorValues deltaDirect = simpleGFG.optimize(); EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); - deltaDirect.print("Direct"); + //deltaDirect.print("Direct"); - // Solve the system using PCG - // With Dummy preconditioner + // Solve the system using Preconditioned Conjugate Gradient solver + // Common PCG parameters 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"); + + // With Dummy preconditioner + pcg->preconditioner_ = boost::make_shared(); VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); - deltaPCGDummy.print("PCG Dummy"); + //deltaPCGDummy.print("PCG Dummy"); // With Block-Jacobi preconditioner - gtsam::PCGSolverParameters::shared_ptr pcgJacobi = boost::make_shared(); - pcgJacobi->preconditioner_ = boost::make_shared(); - pcgJacobi->setMaxIterations(1500);// It takes more than 1000 iterations for this test - pcgJacobi->setEpsilon_abs(0.0); - pcgJacobi->setEpsilon_rel(0.0); - VectorValues deltaPCGJacobi = PCGSolver(*pcgJacobi).optimize(simpleGFG); + pcg->preconditioner_ = boost::make_shared(); + // It takes more than 1000 iterations for this test + pcg->setMaxIterations(1500); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); - // Failed! EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); - deltaPCGJacobi.print("PCG Jacobi"); + //deltaPCGJacobi.print("PCG Jacobi"); } /* ************************************************************************* */ @@ -89,45 +87,38 @@ TEST(PCGSolver, simpleLinearSystem) { 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); - //simpleGFG.print("Factors\n"); // Expected solution VectorValues expectedSolution; expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished()); expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished()); expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished()); + //expectedSolution.print("Expected"); // Solve the system using direct method VectorValues deltaDirect = simpleGFG.optimize(); EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); - expectedSolution.print("Expected"); - deltaDirect.print("Direct"); + //deltaDirect.print("Direct"); - // Solve the system using PCG - VectorValues initial; - initial.insert(0, (Vector(2) << 0.1, -0.1).finished()); - initial.insert(1, (Vector(2) << -0.1, 0.1).finished()); - initial.insert(2, (Vector(2) << -0.1, -0.1).finished()); - - // With Dummy preconditioner + // Solve the system using Preconditioned Conjugate Gradient solver + // Common PCG parameters 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! + // With Dummy preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); - deltaPCGDummy.print("PCG Dummy"); + //deltaPCGDummy.print("PCG Dummy"); - // Solve the system using Preconditioned Conjugate Gradient + // With Block-Jacobi preconditioner pcg->preconditioner_ = boost::make_shared(); - VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); - // Failed! + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); - deltaPCGJacobi.print("PCG Jacobi"); + //deltaPCGJacobi.print("PCG Jacobi"); } From 1c9a1f7cdb7e3e2453a7294b507b4c5839f2b44c Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 5 Dec 2014 15:33:22 -0500 Subject: [PATCH 21/24] Add the citation of the reference. --- gtsam/linear/ConjugateGradientSolver.h | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index ddb6e26bb..5bf793505 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -93,6 +93,10 @@ public: * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. + * + ** REFERENCES: + * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, + * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. */ template V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { @@ -101,8 +105,8 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju estimate = residual = direction = q1 = q2 = initial; system.residual(estimate, q1); /* q1 = b-Ax */ - system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ - system.rightPrecondition(residual, direction);/* d = S^{-1} r */ + system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ + system.rightPrecondition(residual, direction);/* p = L^{-T} r */ double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta; @@ -125,20 +129,20 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju if ( k % iReset == 0 ) { system.residual(estimate, q1); /* q1 = b-Ax */ system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ - system.rightPrecondition(residual, direction); /* d = L^{-T} r */ + system.rightPrecondition(residual, direction); /* p = L^{-T} r */ currentGamma = system.dot(residual, residual); } - system.multiply(direction, q1); /* q1 = A d */ - alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */ - system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */ + system.multiply(direction, q1); /* q1 = A p */ + alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */ + system.axpy(alpha, direction, estimate); /* estimate += alpha * p */ system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */ - system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */ + system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */ prevGamma = currentGamma; - currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */ + currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */ beta = currentGamma / prevGamma; - system.rightPrecondition(residual, q1); /* q1 = L^{-T} residual */ + system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */ system.scal(beta, direction); - system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */ + system.axpy(1.0, q1, direction); /* p = q1 + beta * p */ if (parameters.verbosity() >= ConjugateGradientParameters::ERROR ) std::cout << "[PCG] k = " << k From 47396871a3d0b2a3158671a53ccd9e4ccc5809a4 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 5 Dec 2014 15:43:26 -0500 Subject: [PATCH 22/24] Slightly modify some comments --- gtsam/linear/PCGSolver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 23f4485f9..02f8097ce 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -79,7 +79,7 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { /*****************************************************************************/ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { - /* implement A^t*A*x, assume x and AtAx are pre-allocated */ + /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ // Build a VectorValues for Vector x VectorValues vvX = buildVectorValues(x,keyInfo_); @@ -99,7 +99,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { void GaussianFactorGraphSystem::getb(Vector &b) const { /* compute rhs, assume b pre-allocated */ - // Get whitened r.h.s (b vector) from each factor in the form of VectorValues + // Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues VectorValues vvb = gfg_.gradientAtZero(); // Make the result as Vector form From c5b4d731ccd89034d9a5d8bfaa116600e1ca7e77 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 8 Dec 2014 18:03:40 -0500 Subject: [PATCH 23/24] Correct Yond-Dian's name --- gtsam/linear/ConjugateGradientSolver.cpp | 2 +- gtsam/linear/ConjugateGradientSolver.h | 2 +- gtsam/linear/PCGSolver.cpp | 11 ++--------- gtsam/linear/Preconditioner.cpp | 3 ++- gtsam/linear/Preconditioner.h | 12 +++++++----- 5 files changed, 13 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 0505f6c01..5e7e08f61 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -2,7 +2,7 @@ * ConjugateGradientSolver.cpp * * Created on: Jun 4, 2014 - * Author: ydjian + * Author: Yong-Dian Jian */ #include diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 5bf793505..91b58c2d4 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -12,8 +12,8 @@ /** * @file ConjugateGradientSolver.h * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian * @author Sungtae An - * @author ydjian * @date Nov 6, 2014 **/ diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 02f8097ce..ffb744239 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -2,21 +2,14 @@ * PCGSolver.cpp * * Created on: Feb 14, 2012 - * Author: ydjian + * Author: Yong-Dian Jian * Author: Sungtae An */ #include -//#include -//#include -//#include -//#include #include #include -//#include -//#include -//#include -//#include + #include #include #include diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 576274de2..cef419c3c 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -2,7 +2,8 @@ * Preconditioner.cpp * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #include diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 10ceb78a9..623b29863 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -2,7 +2,8 @@ * Preconditioner.h * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #pragma once @@ -57,7 +58,8 @@ struct GTSAM_EXPORT PreconditionerParameters { }; /* PCG aims to solve the problem: A x = b by reparametrizing it as - * S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M + * L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b, + * where A \approx L L^{T}, or A \approx M * The goal of this class is to provide a general interface to all preconditioners */ class GTSAM_EXPORT Preconditioner { public: @@ -70,15 +72,15 @@ public: /* Computation Interfaces */ - /* implement x = S^{-1} y */ + /* implement x = L^{-1} y */ virtual void solve(const Vector& y, Vector &x) const = 0; // virtual void solve(const VectorValues& y, VectorValues &x) const = 0; - /* implement x = S^{-T} y */ + /* implement x = L^{-T} y */ virtual void transposeSolve(const Vector& y, Vector& x) const = 0; // virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; -// /* implement x = S^{-1} S^{-T} y */ +// /* implement x = L^{-1} L^{-T} y */ // virtual void fullSolve(const Vector& y, Vector &x) const = 0; // virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; From e9b0f7b98fb57bb5c36e7f2307f42dc3067be50e Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 10 Dec 2014 02:55:40 -0500 Subject: [PATCH 24/24] Change Cholesky decomposed matrix from 'Upper' to 'Lower' in order to avoid confusion. --- gtsam/linear/Preconditioner.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index cef419c3c..4c3006a3f 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -95,7 +95,7 @@ void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const { const Eigen::Map R(ptr, d, d); Eigen::Map b(dst, d, 1); - R.triangularView().solveInPlace(b); + R.triangularView().solveInPlace(b); dst += d; ptr += sz; @@ -158,12 +158,12 @@ void BlockJacobiPreconditioner::build( double *ptr = buffer_; for ( size_t i = 0 ; i < n ; ++i ) { /* use eigen to decompose Di */ - /* It is same as R = chol(M) in MATLAB where M is full preconditioner */ - const Matrix R = blocks[i].llt().matrixL().transpose(); + /* It is same as L = chol(M,'lower') in MATLAB where M is full preconditioner */ + const Matrix L = blocks[i].llt().matrixL(); /* store the data in the buffer */ size_t sz = dims_[i]*dims_[i] ; - std::copy(R.data(), R.data() + sz, ptr); + std::copy(L.data(), L.data() + sz, ptr); /* advance the pointer */ ptr += sz;