12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove tests associated with older version (Yong-Dian) of BlockJacobiPreconditioner::build

release/4.3a0
Sungtae An 2014-12-04 22:57:44 -05:00
parent 7760a802ab
commit 558bee685e
1 changed files with 0 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

193
tests/testPreconditioner.cpp
View File

@ -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<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo)
{
const size_t n = keyInfo.size();
std::vector<size_t> dims_ = keyInfo.colSpec();
/* prepare the buffer of block diagonals */
std::vector<Matrix> 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<JacobianFactor>(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<HessianFactor>(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<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
// Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
std::vector<Matrix> 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<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
std::vector<Matrix>::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<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
// Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
std::vector<Matrix> 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<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
std::vector<Matrix>::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<Key, Matrix> expectedHessian =simpleGFG.hessianBlockDiagonal();
// Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
std::vector<Matrix> 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<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
std::vector<Matrix>::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<Key, Matrix> expectedHessian =simpleGFG.hessianBlockDiagonal();
// Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
std::vector<Matrix> 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<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
std::vector<Matrix>::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) {