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();