diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index f00509c3b..bbbb454f4 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include using namespace gtsam; @@ -95,7 +95,7 @@ int main(int argc, char* argv[]) { 0.,0.,-1.), Point3(0.,0.,2.0))); /* 4. Optimize the graph using Levenberg-Marquardt*/ - Values result = optimize (graph, initial); + Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized(); result.print("Final result: "); return 0; diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index 1ff3a8677..b660a1c16 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -20,7 +20,7 @@ // pull in the planar SLAM domain with all typedefs and helper functions defined #include -#include +#include using namespace std; using namespace gtsam; @@ -83,7 +83,7 @@ int main(int argc, char** argv) { initialEstimate.print("initial estimate"); // optimize using Levenberg-Marquardt optimization with an ordering from colamd - planarSLAM::Values result = optimize(graph, initialEstimate); + planarSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized(); result.print("final result"); return 0; diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 10802ee87..46f81d94d 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -35,17 +35,13 @@ // implementations for structures - needed if self-contained, and these should be included last #include -#include +#include #include #include using namespace std; using namespace gtsam; -// Main typedefs -typedef NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain -typedef NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain - /** * In this version of the system we make the following assumptions: * - All values are axis aligned @@ -117,22 +113,27 @@ int main(int argc, char** argv) { // optimize using Levenberg-Marquardt optimization with an ordering from colamd // first using sequential elimination - OptimizerSeqential::shared_values resultSequential = OptimizerSeqential::optimizeLM(*graph, *initial); + LevenbergMarquardtParams lmParams; + lmParams.elimination = LevenbergMarquardtParams::SEQUENTIAL; + Values::const_shared_ptr resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimized(); resultSequential->print("final result (solved with a sequential solver)"); // then using multifrontal, advanced interface - // Note how we create an optimizer, call LM, then we get access to covariances - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial); - OptimizerMultifrontal optimizerMF(graph, initial, ordering); - OptimizerMultifrontal resultMF = optimizerMF.levenbergMarquardt(); - resultMF.values()->print("final result (solved with a multifrontal solver)"); + // Note that we keep the original optimizer object so we can use the COLAMD + // ordering it computes. + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values::const_shared_ptr resultMultifrontal = optimizer.optimized(); + resultMultifrontal->print("final result (solved with a multifrontal solver)"); + + const Ordering& ordering = *optimizer.ordering(); + GaussianMultifrontalSolver linearSolver(*graph->linearize(*resultMultifrontal, ordering)); // Print marginals covariances for all variables - print(resultMF.marginalCovariance(x1), "x1 covariance"); - print(resultMF.marginalCovariance(x2), "x2 covariance"); - print(resultMF.marginalCovariance(x3), "x3 covariance"); - print(resultMF.marginalCovariance(l1), "l1 covariance"); - print(resultMF.marginalCovariance(l2), "l2 covariance"); + print(linearSolver.marginalCovariance(ordering[x1]), "x1 covariance"); + print(linearSolver.marginalCovariance(ordering[x2]), "x2 covariance"); + print(linearSolver.marginalCovariance(ordering[x3]), "x3 covariance"); + print(linearSolver.marginalCovariance(ordering[l1]), "l1 covariance"); + print(linearSolver.marginalCovariance(ordering[l2]), "l2 covariance"); return 0; } diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 1c196488f..c495dba97 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -22,7 +22,7 @@ // pull in the Pose2 SLAM domain with all typedefs and helper functions defined #include -#include +#include #include #include @@ -65,16 +65,16 @@ int main(int argc, char** argv) { Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial); /* 4.2.2 set up solver and optimize */ - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); - Optimizer optimizer(graph, initial, ordering, params); - Optimizer optimizer_result = optimizer.levenbergMarquardt(); - - pose2SLAM::Values result = *optimizer_result.values(); + LevenbergMarquardtParams params; + params.relativeErrorTol = 1e-15; + params.absoluteErrorTol = 1e-15; + pose2SLAM::Values result = *LevenbergMarquardtOptimizer(graph, initial, params, ordering).optimized(); result.print("final result"); /* Get covariances */ - Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1)); - Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(1)); + GaussianMultifrontalSolver solver(*graph->linearize(result, *ordering)); + Matrix covariance1 = solver.marginalCovariance(ordering->at(PoseKey(1))); + Matrix covariance2 = solver.marginalCovariance(ordering->at(PoseKey(1))); print(covariance1, "Covariance1"); print(covariance2, "Covariance2"); diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index 2593c043c..4a4b7aa4f 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -24,7 +24,7 @@ // pull in the Pose2 SLAM domain with all typedefs and helper functions defined #include -#include +#include using namespace std; using namespace gtsam; @@ -61,7 +61,7 @@ int main(int argc, char** argv) { /* 4 Single Step Optimization * optimize using Levenberg-Marquardt optimization with an ordering from colamd */ - pose2SLAM::Values result = optimize(graph, initial); + pose2SLAM::Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized(); result.print("final result"); diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 71727f750..f3722da79 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include /* * TODO: make factors independent of RotValues @@ -105,7 +105,7 @@ int main() { * initial estimate. This will yield a new RotValues structure * with the final state of the optimization. */ - Values result = optimize(graph, initial); + Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized(); result.print("final result"); return 0; diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index c7e0df05c..076430858 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -20,7 +20,7 @@ using namespace boost; #include -#include +#include #include #include @@ -143,12 +143,13 @@ int main(int argc, char* argv[]) { // Optimize the graph cout << "*******************************************************" << endl; - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newVerbosity(Optimizer::Parameters::DAMPED); - visualSLAM::Optimizer::shared_values result = visualSLAM::Optimizer::optimizeGN(graph, initialEstimates, params); + LevenbergMarquardtParams params; + params.lmVerbosity = LevenbergMarquardtParams::DAMPED; + visualSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimized(); // Print final results cout << "*******************************************************" << endl; - result->print("FINAL RESULTS: "); + result.print("FINAL RESULTS: "); return 0; }