From 2c8d8dbde4a885635be3d8f991836de3db3dc56d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 11 Jan 2010 20:17:28 +0000 Subject: [PATCH] NonlinearOptimizer constructor now requires shared_ptr to prevent storing pointers to stack and temporary variables. Code that uses it will need to be modified, but CitySLAM is already done. --- cpp/NonlinearOptimizer-inl.h | 12 ++++---- cpp/NonlinearOptimizer.h | 21 +++++++++---- cpp/testNonlinearOptimizer.cpp | 33 +++++++++++---------- cpp/testPose2Graph.cpp | 39 ++++++++++++------------ cpp/testPose3Graph.cpp | 25 ++++++++-------- cpp/testSQP.cpp | 54 ++++++++++++++++++---------------- cpp/testVSLAMGraph.cpp | 28 ++++++++++-------- 7 files changed, 115 insertions(+), 97 deletions(-) diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index c53f2e5c6..ed2776ad8 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -44,9 +44,9 @@ namespace gtsam { // Constructors /* ************************************************************************* */ template - NonlinearOptimizer::NonlinearOptimizer(const G& graph, - const Ordering& ordering, shared_config config, double lambda) : - graph_(&graph), ordering_(&ordering), config_(config), error_(graph.error( + NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, + shared_ordering ordering, shared_config config, double lambda) : + graph_(graph), ordering_(ordering), config_(config), error_(graph->error( *config)), lambda_(lambda) { } @@ -88,7 +88,7 @@ namespace gtsam { if (verbosity >= CONFIG) newConfig->print("newConfig"); - return NonlinearOptimizer(*graph_, *ordering_, newConfig); + return NonlinearOptimizer(graph_, ordering_, newConfig); } /* ************************************************************************* */ @@ -139,14 +139,14 @@ namespace gtsam { newConfig->print("config"); // create new optimization state with more adventurous lambda - NonlinearOptimizer next(*graph_, *ordering_, newConfig, lambda_ / factor); + NonlinearOptimizer next(graph_, ordering_, newConfig, lambda_ / factor); // if error decreased, return the new state if (next.error_ <= error_) return next; else { // TODO: can we avoid copying the config ? - NonlinearOptimizer cautious(*graph_, *ordering_, config_, lambda_ * factor); + NonlinearOptimizer cautious(graph_, ordering_, config_, lambda_ * factor); return cautious.try_lambda(linear, verbosity, factor); } } diff --git a/cpp/NonlinearOptimizer.h b/cpp/NonlinearOptimizer.h index 36a99b17a..720195b52 100644 --- a/cpp/NonlinearOptimizer.h +++ b/cpp/NonlinearOptimizer.h @@ -31,6 +31,8 @@ namespace gtsam { // For performance reasons in recursion, we store configs in a shared_ptr typedef boost::shared_ptr shared_config; + typedef boost::shared_ptr shared_graph; + typedef boost::shared_ptr shared_ordering; enum verbosityLevel { SILENT, @@ -49,17 +51,17 @@ namespace gtsam { // keep a reference to const versions of the graph and ordering // These normally do not change - const FactorGraph* graph_; - const Ordering* ordering_; + const shared_graph graph_; + const shared_ordering ordering_; // keep a configuration and its error // These typically change once per iteration (in a functional way) - shared_config config_; - double error_; + const shared_config config_; + const double error_; // keep current lambda for use within LM only // TODO: red flag, should we have an LM class ? - double lambda_; + const double lambda_; // Recursively try to do tempered Gauss-Newton steps until we succeed NonlinearOptimizer try_lambda(const GaussianFactorGraph& linear, @@ -70,9 +72,16 @@ namespace gtsam { /** * Constructor */ - NonlinearOptimizer(const FactorGraph& graph, const Ordering& ordering, + NonlinearOptimizer(shared_graph graph, shared_ordering ordering, shared_config config, double lambda = 1e-5); + /** + * Copy constructor + */ + NonlinearOptimizer(const NonlinearOptimizer &optimizer) : + graph_(optimizer.graph_), ordering_(optimizer.ordering_), config_(optimizer.config_), + error_(optimizer.error_), lambda_(optimizer.lambda_) {} + /** * Return current error */ diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 415560f7f..e7c7afc6c 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -12,6 +12,9 @@ using namespace boost::assign; #include +#include +using namespace boost; + #include "Matrix.h" #include "Ordering.h" #include "smallExample.h" @@ -27,7 +30,7 @@ typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( NonlinearOptimizer, delta ) { - ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); + shared_ptr fg(new ExampleNonlinearFactorGraph(createNonlinearFactorGraph())); Optimizer::shared_config initial = sharedNoisyConfig(); // Expected configuration is the difference between the noisy config @@ -47,22 +50,22 @@ TEST( NonlinearOptimizer, delta ) expected.insert("x2", dx2); // Check one ordering - Ordering ord1; - ord1 += "x2","l1","x1"; + shared_ptr ord1(new Ordering()); + *ord1 += "x2","l1","x1"; Optimizer optimizer1(fg, ord1, initial); VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual1,expected)); // Check another - Ordering ord2; - ord2 += "x1","x2","l1"; + shared_ptr ord2(new Ordering()); + *ord2 += "x1","x2","l1"; Optimizer optimizer2(fg, ord2, initial); VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual2,expected)); // And yet another... - Ordering ord3; - ord3 += "l1","x1","x2"; + shared_ptr ord3(new Ordering()); + *ord3 += "l1","x1","x2"; Optimizer optimizer3(fg, ord3, initial); VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual3,expected)); @@ -72,7 +75,7 @@ TEST( NonlinearOptimizer, delta ) TEST( NonlinearOptimizer, iterateLM ) { // really non-linear factor graph - ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); + shared_ptr fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph())); // config far from minimum Vector x0 = Vector_(1, 3.0); @@ -80,8 +83,8 @@ TEST( NonlinearOptimizer, iterateLM ) config->insert("x", x0); // ordering - Ordering ord; - ord.push_back("x"); + shared_ptr ord(new Ordering()); + ord->push_back("x"); // create initial optimization state, with lambda=0 Optimizer optimizer(fg, ord, config, 0); @@ -107,23 +110,23 @@ TEST( NonlinearOptimizer, iterateLM ) /* ************************************************************************* */ TEST( NonlinearOptimizer, optimize ) { - ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); + shared_ptr fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph())); // test error at minimum Vector xstar = Vector_(1, 0.0); VectorConfig cstar; cstar.insert("x", xstar); - DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); + DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Vector x0 = Vector_(1, 3.0); boost::shared_ptr c0(new VectorConfig); c0->insert("x", x0); - DOUBLES_EQUAL(199.0,fg.error(*c0),1e-3); + DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); // optimize parameters - Ordering ord; - ord.push_back("x"); + shared_ptr ord(new Ordering()); + ord->push_back("x"); double relativeThreshold = 1e-5; double absoluteThreshold = 1e-5; diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2Graph.cpp index bda87c0a0..56321d01d 100644 --- a/cpp/testPose2Graph.cpp +++ b/cpp/testPose2Graph.cpp @@ -6,6 +6,7 @@ #include #include #include +using namespace boost; using namespace boost::assign; #include @@ -84,9 +85,9 @@ TEST( Pose2Graph, linerization ) TEST(Pose2Graph, optimize) { // create a Pose graph with one equality constraint and one measurement - Pose2Graph fg; - fg.addConstraint("p0", Pose2(0,0,0)); - fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance); + shared_ptr fg(new Pose2Graph); + fg->addConstraint("p0", Pose2(0,0,0)); + fg->add("p0", "p1", Pose2(1,2,M_PI_2), covariance); // Create initial config boost::shared_ptr initial(new Pose2Config()); @@ -94,13 +95,13 @@ TEST(Pose2Graph, optimize) { initial->insert("p1", Pose2(0,0,0)); // Choose an ordering and optimize - Ordering ordering; - ordering += "p0","p1"; + shared_ptr ordering(new Ordering); + *ordering += "p0","p1"; typedef NonlinearOptimizer Optimizer; - Optimizer optimizer(fg, ordering, initial); + Optimizer optimizer0(fg, ordering, initial); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; //Optimizer::verbosityLevel verbosity = Optimizer::ERROR; - optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); + Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); // Check with expected config Pose2Config expected; @@ -118,15 +119,15 @@ TEST(Pose2Graph, optimizeCircle) { Pose2 p0 = hexagon["p0"], p1 = hexagon["p1"]; // create a Pose graph with one equality constraint and one measurement - Pose2Graph fg; - fg.addConstraint("p0", p0); + shared_ptr fg(new Pose2Graph); + fg->addConstraint("p0", p0); Pose2 delta = between(p0,p1); - fg.add("p0", "p1", delta, covariance); - fg.add("p1", "p2", delta, covariance); - fg.add("p2", "p3", delta, covariance); - fg.add("p3", "p4", delta, covariance); - fg.add("p4", "p5", delta, covariance); - fg.add("p5", "p0", delta, covariance); + fg->add("p0", "p1", delta, covariance); + fg->add("p1", "p2", delta, covariance); + fg->add("p2", "p3", delta, covariance); + fg->add("p3", "p4", delta, covariance); + fg->add("p4", "p5", delta, covariance); + fg->add("p5", "p0", delta, covariance); // Create initial config boost::shared_ptr initial(new Pose2Config()); @@ -138,13 +139,13 @@ TEST(Pose2Graph, optimizeCircle) { initial->insert("p5", expmap(hexagon["p5"],Vector_(3,-0.1, 0.1,-0.1))); // Choose an ordering and optimize - Ordering ordering; - ordering += "p0","p1","p2","p3","p4","p5"; + shared_ptr ordering(new Ordering); + *ordering += "p0","p1","p2","p3","p4","p5"; typedef NonlinearOptimizer Optimizer; - Optimizer optimizer(fg, ordering, initial); + Optimizer optimizer0(fg, ordering, initial); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; // Optimizer::verbosityLevel verbosity = Optimizer::ERROR; - optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); + Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); Pose2Config actual = *optimizer.config(); diff --git a/cpp/testPose3Graph.cpp b/cpp/testPose3Graph.cpp index fde8b98a2..05529011f 100644 --- a/cpp/testPose3Graph.cpp +++ b/cpp/testPose3Graph.cpp @@ -8,6 +8,7 @@ #include #include #include +using namespace boost; using namespace boost::assign; #include @@ -32,15 +33,15 @@ TEST(Pose3Graph, optimizeCircle) { Pose3 p0 = hexagon["p0"], p1 = hexagon["p1"]; // create a Pose graph with one equality constraint and one measurement - Pose3Graph fg; - fg.addConstraint("p0", p0); + shared_ptr fg(new Pose3Graph); + fg->addConstraint("p0", p0); Pose3 delta = between(p0,p1); - fg.add("p0", "p1", delta, covariance); - fg.add("p1", "p2", delta, covariance); - fg.add("p2", "p3", delta, covariance); - fg.add("p3", "p4", delta, covariance); - fg.add("p4", "p5", delta, covariance); - fg.add("p5", "p0", delta, covariance); + fg->add("p0", "p1", delta, covariance); + fg->add("p1", "p2", delta, covariance); + fg->add("p2", "p3", delta, covariance); + fg->add("p3", "p4", delta, covariance); + fg->add("p4", "p5", delta, covariance); + fg->add("p5", "p0", delta, covariance); // Create initial config boost::shared_ptr initial(new Pose3Config()); @@ -52,13 +53,13 @@ TEST(Pose3Graph, optimizeCircle) { initial->insert("p5", expmap(hexagon["p5"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize - Ordering ordering; - ordering += "p0","p1","p2","p3","p4","p5"; + shared_ptr ordering(new Ordering); + *ordering += "p0","p1","p2","p3","p4","p5"; typedef NonlinearOptimizer Optimizer; - Optimizer optimizer(fg, ordering, initial); + Optimizer optimizer0(fg, ordering, initial); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; // Optimizer::verbosityLevel verbosity = Optimizer::ERROR; - optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); + Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); Pose3Config actual = *optimizer.config(); diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 017ee0c14..532b3fb96 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -9,6 +9,7 @@ #include // for operator += #include // for insert #include +#include #include #include #include @@ -32,6 +33,7 @@ using namespace std; using namespace gtsam; +using namespace boost; using namespace boost::assign; // trick from some reading group @@ -286,11 +288,11 @@ TEST (SQP, two_pose_truth ) { shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1")); // construct the graph - NLGraph graph; - graph.push_back(ef1); - graph.push_back(ef2); - graph.push_back(f1); - graph.push_back(f2); + shared_ptr graph(new NLGraph()); + graph->push_back(ef1); + graph->push_back(ef2); + graph->push_back(f1); + graph->push_back(f2); // create an initial estimate boost::shared_ptr initialEstimate(new VectorConfig(feas)); // must start with feasible set @@ -298,8 +300,8 @@ TEST (SQP, two_pose_truth ) { //initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error // optimize the graph - Ordering ordering; - ordering += "x1", "x2", "l1"; + shared_ptr ordering(new Ordering()); + *ordering += "x1", "x2", "l1"; Optimizer optimizer(graph, ordering, initialEstimate, 1e-5); // display solution @@ -498,27 +500,27 @@ TEST (SQP, stereo_truth ) { truthConfig->addLandmarkPoint(1, landmark); // create graph - VSLAMGraph graph; + shared_ptr graph(new VSLAMGraph()); // create equality constraints for poses - graph.addCameraConstraint(1, camera1.pose()); - graph.addCameraConstraint(2, camera2.pose()); + graph->addCameraConstraint(1, camera1.pose()); + graph->addCameraConstraint(2, camera2.pose()); // create VSLAM factors Point2 z1 = camera1.project(landmark); if (verbose) z1.print("z1"); shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK)); - graph.push_back(vf1); + graph->push_back(vf1); Point2 z2 = camera2.project(landmark); if (verbose) z2.print("z2"); shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK)); - graph.push_back(vf2); + graph->push_back(vf2); - if (verbose) graph.print("Graph after construction"); + if (verbose) graph->print("Graph after construction"); // create ordering - Ordering ord; - ord += "x1", "x2", "l1"; + shared_ptr ord(new Ordering()); + *ord += "x1", "x2", "l1"; // create optimizer VOptimizer optimizer(graph, ord, truthConfig, 1e-5); @@ -571,40 +573,40 @@ TEST (SQP, stereo_truth_noisy ) { noisyConfig->addLandmarkPoint(1, landmarkNoisy); // create graph - VSLAMGraph graph; + shared_ptr graph(new VSLAMGraph()); // create equality constraints for poses - graph.addCameraConstraint(1, camera1.pose()); - graph.addCameraConstraint(2, camera2.pose()); + graph->addCameraConstraint(1, camera1.pose()); + graph->addCameraConstraint(2, camera2.pose()); // create VSLAM factors Point2 z1 = camera1.project(landmark); if (verbose) z1.print("z1"); shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK)); - graph.push_back(vf1); + graph->push_back(vf1); Point2 z2 = camera2.project(landmark); if (verbose) z2.print("z2"); shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK)); - graph.push_back(vf2); + graph->push_back(vf2); if (verbose) { - graph.print("Graph after construction"); + graph->print("Graph after construction"); noisyConfig->print("Initial config"); } // create ordering - Ordering ord; - ord += "x1", "x2", "l1"; + shared_ptr ord(new Ordering()); + *ord += "x1", "x2", "l1"; // create optimizer - VOptimizer optimizer(graph, ord, noisyConfig, 1e-5); + VOptimizer optimizer0(graph, ord, noisyConfig, 1e-5); if (verbose) - cout << "Initial Error: " << optimizer.error() << endl; + cout << "Initial Error: " << optimizer0.error() << endl; // use Levenberg-Marquardt optimization double relThresh = 1e-5, absThresh = 1e-5; - optimizer = optimizer.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT); + VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT)); // verify DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 0d5ac4517..72415e20a 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -8,6 +8,8 @@ */ #include +#include +using namespace boost; #include "VSLAMGraph.h" #include "NonlinearFactorGraph-inl.h" @@ -66,11 +68,11 @@ VSLAMGraph testGraph() { TEST( VSLAMGraph, optimizeLM) { // build a graph - VSLAMGraph graph = testGraph(); + shared_ptr graph(new VSLAMGraph(testGraph())); // add 3 landmark constraints - graph.addLandmarkConstraint(1, landmark1); - graph.addLandmarkConstraint(2, landmark2); - graph.addLandmarkConstraint(3, landmark3); + graph->addLandmarkConstraint(1, landmark1); + graph->addLandmarkConstraint(2, landmark2); + graph->addLandmarkConstraint(3, landmark3); // Create an initial configuration corresponding to the ground truth boost::shared_ptr initialEstimate(new VSLAMConfig); @@ -89,7 +91,7 @@ TEST( VSLAMGraph, optimizeLM) keys.push_back("l4"); keys.push_back("x1"); keys.push_back("x2"); - Ordering ordering(keys); + shared_ptr ordering(new Ordering(keys)); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -110,10 +112,10 @@ TEST( VSLAMGraph, optimizeLM) TEST( VSLAMGraph, optimizeLM2) { // build a graph - VSLAMGraph graph = testGraph(); + shared_ptr graph(new VSLAMGraph(testGraph())); // add 2 camera constraints - graph.addCameraConstraint(1, camera1); - graph.addCameraConstraint(2, camera2); + graph->addCameraConstraint(1, camera1); + graph->addCameraConstraint(2, camera2); // Create an initial configuration corresponding to the ground truth boost::shared_ptr initialEstimate(new VSLAMConfig); @@ -133,7 +135,7 @@ TEST( VSLAMGraph, optimizeLM2) keys.push_back("l4"); keys.push_back("x1"); keys.push_back("x2"); - Ordering ordering(keys); + shared_ptr ordering(new Ordering(keys)); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -154,10 +156,10 @@ TEST( VSLAMGraph, optimizeLM2) TEST( VSLAMGraph, CHECK_ORDERING) { // build a graph - VSLAMGraph graph = testGraph(); + shared_ptr graph(new VSLAMGraph(testGraph())); // add 2 camera constraints - graph.addCameraConstraint(1, camera1); - graph.addCameraConstraint(2, camera2); + graph->addCameraConstraint(1, camera1); + graph->addCameraConstraint(2, camera2); // Create an initial configuration corresponding to the ground truth boost::shared_ptr initialEstimate(new VSLAMConfig); @@ -168,7 +170,7 @@ TEST( VSLAMGraph, CHECK_ORDERING) initialEstimate->addLandmarkPoint(3, landmark3); initialEstimate->addLandmarkPoint(4, landmark4); - Ordering ordering = graph.getOrdering(); + shared_ptr ordering(new Ordering(graph->getOrdering())); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth