diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index bbbb454f4..7d0e37e7f 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -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 = *LevenbergMarquardtOptimizer(graph, initial).optimized(); + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); result.print("Final result: "); return 0; diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index b660a1c16..912589080 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -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 = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized(); + planarSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); result.print("final result"); return 0; diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 46f81d94d..a7a45b726 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -57,14 +57,14 @@ int main(int argc, char** argv) { Symbol l1('l',1), l2('l',2); // create graph container and add factors to it - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + NonlinearFactorGraph graph; /* add prior */ // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor - graph->add(posePrior); // add the factor to the graph + graph.add(posePrior); // add the factor to the graph /* add odometry */ // general noisemodel for odometry @@ -73,8 +73,8 @@ int main(int argc, char** argv) { // create between factors to represent odometry BetweenFactor odom12(x1, x2, odom_measurement, odom_model); BetweenFactor odom23(x2, x3, odom_measurement, odom_model); - graph->add(odom12); // add both to graph - graph->add(odom23); + graph.add(odom12); // add both to graph + graph.add(odom23); /* add measurements */ // general noisemodel for measurements @@ -94,39 +94,39 @@ int main(int argc, char** argv) { BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); // add the factors - graph->add(meas11); - graph->add(meas21); - graph->add(meas32); + graph.add(meas11); + graph.add(meas21); + graph.add(meas32); - graph->print("Full Graph"); + graph.print("Full Graph"); // initialize to noisy points - boost::shared_ptr initial(new Values); - initial->insert(x1, Pose2(0.5, 0.0, 0.2)); - initial->insert(x2, Pose2(2.3, 0.1,-0.2)); - initial->insert(x3, Pose2(4.1, 0.1, 0.1)); - initial->insert(l1, Point2(1.8, 2.1)); - initial->insert(l2, Point2(4.1, 1.8)); + Values initial; + initial.insert(x1, Pose2(0.5, 0.0, 0.2)); + initial.insert(x2, Pose2(2.3, 0.1,-0.2)); + initial.insert(x3, Pose2(4.1, 0.1, 0.1)); + initial.insert(l1, Point2(1.8, 2.1)); + initial.insert(l2, Point2(4.1, 1.8)); - initial->print("initial estimate"); + initial.print("initial estimate"); // optimize using Levenberg-Marquardt optimization with an ordering from colamd // first using sequential elimination 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)"); + Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); + resultSequential.print("final result (solved with a sequential solver)"); // then using multifrontal, advanced interface // 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)"); + Values resultMultifrontal = optimizer.optimize(); + resultMultifrontal.print("final result (solved with a multifrontal solver)"); - const Ordering& ordering = *optimizer.ordering(); - GaussianMultifrontalSolver linearSolver(*graph->linearize(*resultMultifrontal, ordering)); + const Ordering& ordering = *optimizer.params().ordering; + GaussianMultifrontalSolver linearSolver(*graph.linearize(resultMultifrontal, ordering)); // Print marginals covariances for all variables print(linearSolver.marginalCovariance(ordering[x1]), "x1 covariance"); diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 9cc8c2040..282287827 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -34,13 +34,13 @@ using namespace pose2SLAM; int main(int argc, char** argv) { /* 1. create graph container and add factors to it */ - shared_ptr graph(new Graph); + Graph graph; /* 2.a add prior */ // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph->addPrior(1, prior_measurement, prior_model); // add directly to graph + graph.addPrior(1, prior_measurement, prior_model); // add directly to graph /* 2.b add odometry */ // general noisemodel for odometry @@ -48,36 +48,38 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph->addOdometry(1, 2, odom_measurement, odom_model); - graph->addOdometry(2, 3, odom_measurement, odom_model); - graph->print("full graph"); + graph.addOdometry(1, 2, odom_measurement, odom_model); + graph.addOdometry(2, 3, odom_measurement, odom_model); + graph.print("full graph"); /* 3. Create the data structure to hold the initial estimate to the solution * initialize to noisy points */ - shared_ptr initial(new pose2SLAM::Values); - initial->insertPose(1, Pose2(0.5, 0.0, 0.2)); - initial->insertPose(2, Pose2(2.3, 0.1,-0.2)); - initial->insertPose(3, Pose2(4.1, 0.1, 0.1)); - initial->print("initial estimate"); + pose2SLAM::Values initial; + initial.insertPose(1, Pose2(0.5, 0.0, 0.2)); + initial.insertPose(2, Pose2(2.3, 0.1,-0.2)); + initial.insertPose(3, Pose2(4.1, 0.1, 0.1)); + initial.print("initial estimate"); /* 4.2.1 Alternatively, you can go through the process step by step * Choose an ordering */ - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial); + Ordering 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(); + LevenbergMarquardtParams params; + params.absoluteErrorTol = 1e-15; + params.relativeErrorTol = 1e-15; + params.ordering = ordering; + LevenbergMarquardtOptimizer optimizer(graph, initial, params); - pose2SLAM::Values result = *optimizer_result.values(); + pose2SLAM::Values result = optimizer.optimize(); result.print("final result"); - /* Get covariances */ - Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1)); - Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(2)); - - print(covariance1, "Covariance1"); - print(covariance2, "Covariance2"); +// /* Get covariances */ +// Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1)); +// Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(2)); +// +// print(covariance1, "Covariance1"); +// print(covariance2, "Covariance2"); return 0; } diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index 4a4b7aa4f..18a756aba 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -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 = *LevenbergMarquardtOptimizer(graph, initial).optimized(); + pose2SLAM::Values result = graph.optimize(initial); result.print("final result"); diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index f3722da79..603d345bf 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -105,7 +105,7 @@ int main() { * initial estimate. This will yield a new RotValues structure * with the final state of the optimization. */ - Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized(); + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); result.print("final result"); return 0; diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index 076430858..6edd06465 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -129,23 +129,23 @@ int main(int argc, char* argv[]) { readAllData(); // Create a graph using the 2D measurements (features) and the calibration data - boost::shared_ptr graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); + Graph graph(setupGraph(g_measurements, measurementSigma, g_calib)); // Create an initial Values structure using groundtruth values as the initial estimates - boost::shared_ptr initialEstimates(new Values(initialize(g_landmarks, g_poses))); + Values initialEstimates(initialize(g_landmarks, g_poses)); cout << "*******************************************************" << endl; - initialEstimates->print("INITIAL ESTIMATES: "); + initialEstimates.print("INITIAL ESTIMATES: "); // Add prior factor for all poses in the graph map::iterator poseit = g_poses.begin(); for (; poseit != g_poses.end(); poseit++) - graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1)); + graph.addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1)); // Optimize the graph cout << "*******************************************************" << endl; LevenbergMarquardtParams params; params.lmVerbosity = LevenbergMarquardtParams::DAMPED; - visualSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimized(); + visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimize(); // Print final results cout << "*******************************************************" << endl; diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 321371d41..06994d8bd 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -110,7 +110,7 @@ namespace planarSLAM { /// Optimize Values optimize(const Values& initialEstimate) { - return LevenbergMarquardtOptimizer(*this).optimized(initialEstimate); + return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); } };