diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index fbd2581ef..45a4662d6 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -26,7 +26,7 @@ // For an explanation of these headers, see SFMExample.cpp #include "SFMdata.h" -#include +#include 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, as sometimes - // the triangulation operation inside smart factor will encounter degeneracy. - boost::optional point; - // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - point = smart->point(result); + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. + boost::optional 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; } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 82d7f62c3..472cd9447 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -21,8 +21,8 @@ // These extra headers allow us a LM outer loop with PCG linear solver (inner loop) #include -#include #include +#include 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(); pcg->preconditioner_ = boost::make_shared(); + // 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; }