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

Making Direct and Iterative solvers comparable

release/4.3a0
dellaert 2015-02-18 15:47:06 +01:00
parent 37cc0acbf7
commit fd7411c68c
2 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
examples/SFMExample_SmartFactor.cpp
View File

@ -26,7 +26,7 @@
// For an explanation of these headers, see SFMExample.cpp
#include "SFMdata.h"
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
@ -98,7 +98,8 @@ int main(int argc, char* argv[]) {
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
result.print("Final results:\n");
// A smart factor represent the 3D point inside the factor, not as a variable.
@ -107,20 +108,20 @@ int main(int argc, char* argv[]) {
Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) {
// The output of point() is in boost::optional<gtsam::Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point;
// The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
point = smart->point(result);
// The output of point() is in boost::optional<gtsam::Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
landmark_result.insert(j, *point);
}
}
landmark_result.print("Landmark results:\n");
cout << "final error: " << graph.error(result) << endl;
cout << "number of iterations: " << optimizer.iterations() << endl;
return 0;
}

14
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -21,8 +21,8 @@
// These extra headers allow us a LM outer loop with PCG linear solver (inner loop)
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/PCGSolver.h>
using namespace std;
using namespace gtsam;
@ -88,14 +88,20 @@ int main(int argc, char* argv[]) {
// iterative solver, in this case the SPCG (subgraph PCG)
LevenbergMarquardtParams parameters;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
parameters.absoluteErrorTol = 1e-10;
parameters.relativeErrorTol = 1e-10;
parameters.maxIterations = 500;
PCGSolverParameters::shared_ptr pcg =
boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ =
boost::make_shared<BlockJacobiPreconditionerParameters>();
// Following is crucial:
pcg->setEpsilon_abs(1e-10);
pcg->setEpsilon_rel(1e-10);
parameters.iterativeParams = pcg;
LevenbergMarquardtOptimizer Optimizer(graph, initialEstimate, parameters);
Values result = Optimizer.optimize();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
// Display result as in SFMExample_SmartFactor.run
result.print("Final results:\n");
@ -111,6 +117,8 @@ int main(int argc, char* argv[]) {
}
landmark_result.print("Landmark results:\n");
cout << "final error: " << graph.error(result) << endl;
cout << "number of iterations: " << optimizer.iterations() << endl;
return 0;
}