Clean up the code
							parent
							
								
									cfb82d9a96
								
							
						
					
					
						commit
						06b82ce3e3
					
				|  | @ -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<gtsam::PCGSolverParameters>(); | ||||
|   pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>(); | ||||
|   pcg->setMaxIterations(500); | ||||
|   pcg->setEpsilon_abs(0.0); | ||||
|   pcg->setEpsilon_rel(0.0); | ||||
|   //pcg->setVerbosity("ERROR");
 | ||||
| 
 | ||||
|   // With Dummy preconditioner
 | ||||
|   pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>(); | ||||
|   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<gtsam::PCGSolverParameters>(); | ||||
|   pcgJacobi->preconditioner_ = boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>(); | ||||
|   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<gtsam::BlockJacobiPreconditionerParameters>(); | ||||
|   // 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<gtsam::PCGSolverParameters>(); | ||||
|   pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>(); | ||||
|   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<Key,Vector>(), initial); | ||||
|   // Failed!
 | ||||
|   // With Dummy preconditioner
 | ||||
|   pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>(); | ||||
|   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<gtsam::BlockJacobiPreconditionerParameters>(); | ||||
|   VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map<Key,Vector>(), initial); | ||||
|   // Failed!
 | ||||
|   VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); | ||||
|   EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); | ||||
|   deltaPCGJacobi.print("PCG Jacobi"); | ||||
|   //deltaPCGJacobi.print("PCG Jacobi");
 | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue