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

Add a different test for building blocks

release/4.3a0
Sungtae An 2014-11-14 14:09:42 -05:00
parent 2733b66a23
commit ee3c7ce182
1 changed files with 37 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
tests/testPreconditioner.cpp
View File

@ -42,6 +42,21 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
return fg;
}
/* ************************************************************************* */
static GaussianFactorGraph createSimpleGaussianFactorGraphUnordered() {
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(2, -10*eye(2), 1, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);
return fg;
}
/* ************************************************************************* */
// Copy of BlockJacobiPreconditioner::build
std::vector<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo)
@ -89,10 +104,6 @@ std::vector<Matrix> 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<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
@ -110,6 +121,28 @@ TEST( Preconditioner, buildBlocks ) {
EXPECT(assert_equal(it1->second, *it2));
}
/* ************************************************************************* */
TEST( Preconditioner, buildBlocks2 ) {
// Create simple Gaussian factor graph and initial values
GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered();
// Expected Hessian block diagonal matrices
std::map<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
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));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */