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

temporary printing out for test 

Fix Eigen comma initialization
release/4.3a0
Sungtae An 2014-12-01 05:07:43 -05:00
parent b601eb0f92
commit f3bbe604d6
1 changed files with 31 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
tests/testPreconditioner.cpp
View File

@ -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<gtsam::BlockJacobiPreconditionerParameters>());
preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map<Key,Vector>());
boost::shared_ptr<BlockJacobiPreconditioner> blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditioner>(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<Key, Matrix> 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<gtsam::PCGSolverParameters>();