From 3bf15333af9ba4c68fc2b0ff1bac7f1827ed252f Mon Sep 17 00:00:00 2001 From: Kai Ni Date: Wed, 20 Jan 2010 02:28:23 +0000 Subject: [PATCH] move ordering into the solver, and the nonlinear optimizer is now exact --- cpp/GaussianFactorGraph.h | 11 +- cpp/NonlinearOptimizer-inl.h | 14 +- cpp/NonlinearOptimizer.h | 10 +- cpp/Pose2.h | 2 + cpp/SubgraphPreconditioner-inl.h | 6 +- cpp/SubgraphPreconditioner.h | 5 +- cpp/testNonlinearOptimizer.cpp | 303 ++++++++++++++++--------------- cpp/testPose2SLAM.cpp | 6 +- cpp/testPose3SLAM.cpp | 3 +- cpp/testSQP.cpp | 9 +- cpp/testVSLAMGraph.cpp | 9 +- 11 files changed, 193 insertions(+), 185 deletions(-) diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 1b1b3f530..4a491a6c2 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -228,17 +228,18 @@ namespace gtsam { */ template class Factorization { - public: - Factorization() {} + private: + boost::shared_ptr ordering_; - Factorization(const NonlinearGraph& g, const Config& config) {} + public: + Factorization(boost::shared_ptr ordering) : ordering_(ordering) {} /** * solve for the optimal displacement in the tangent space, and then solve * the resulted linear system */ - VectorConfig optimize(GaussianFactorGraph& fg, const Ordering& ordering) const { - return fg.optimize(ordering); + VectorConfig optimize(GaussianFactorGraph& fg) const { + return fg.optimize(*ordering_); } /** diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index 7641b7acf..2bd01a8d5 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -48,8 +48,8 @@ namespace gtsam { /* ************************************************************************* */ template NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, - shared_ordering ordering, shared_config config, shared_solver solver, double lambda) : - graph_(graph), ordering_(ordering), config_(config), error_(graph->error( + shared_config config, shared_solver solver, double lambda) : + graph_(graph), config_(config), error_(graph->error( *config)), lambda_(lambda), solver_(solver) { } @@ -59,7 +59,7 @@ namespace gtsam { template VectorConfig NonlinearOptimizer::linearizeAndOptimizeForDelta() const { L linearized = solver_->linearize(*graph_, *config_); - return solver_->optimize(linearized, *ordering_); + return solver_->optimize(linearized); } /* ************************************************************************* */ @@ -82,7 +82,7 @@ namespace gtsam { if (verbosity >= CONFIG) newConfig->print("newConfig"); - NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, ordering_, newConfig, solver_); + NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newConfig, solver_); if (verbosity >= ERROR) cout << "error: " << newOptimizer.error_ << endl; @@ -128,7 +128,7 @@ namespace gtsam { damped.print("damped"); // solve - VectorConfig delta = solver_->optimize(damped, *ordering_); + VectorConfig delta = solver_->optimize(damped); if (verbosity >= TRYDELTA) delta.print("delta"); @@ -138,14 +138,14 @@ namespace gtsam { newConfig->print("config"); // create new optimization state with more adventurous lambda - NonlinearOptimizer next(graph_, ordering_, newConfig, solver_, lambda_ / factor); + NonlinearOptimizer next(graph_, newConfig, solver_, 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_, solver_, lambda_ * factor); + NonlinearOptimizer cautious(graph_, config_, solver_, lambda_ * factor); return cautious.try_lambda(linear, verbosity, factor); } } diff --git a/cpp/NonlinearOptimizer.h b/cpp/NonlinearOptimizer.h index 5b208070f..df775701c 100644 --- a/cpp/NonlinearOptimizer.h +++ b/cpp/NonlinearOptimizer.h @@ -33,8 +33,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; typedef boost::shared_ptr shared_solver; + typedef const S solver; enum verbosityLevel { SILENT, @@ -51,10 +51,9 @@ namespace gtsam { private: - // keep a reference to const versions of the graph and ordering + // keep a reference to const version of the graph // These normally do not change const shared_graph graph_; - const shared_ordering ordering_; // keep a configuration and its error // These typically change once per iteration (in a functional way) @@ -77,15 +76,14 @@ namespace gtsam { /** * Constructor */ - NonlinearOptimizer(shared_graph graph, shared_ordering ordering, - shared_config config, shared_solver solver = shared_solver(new S), + NonlinearOptimizer(shared_graph graph, shared_config config, shared_solver solver, double lambda = 1e-5); /** * Copy constructor */ NonlinearOptimizer(const NonlinearOptimizer &optimizer) : - graph_(optimizer.graph_), ordering_(optimizer.ordering_), config_(optimizer.config_), + graph_(optimizer.graph_), config_(optimizer.config_), error_(optimizer.error_), lambda_(optimizer.lambda_), solver_(optimizer.solver_) {} /** diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 5bb443c47..2f1298f13 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -59,6 +59,8 @@ namespace gtsam { Point2 t() const { return t_; } Rot2 r() const { return r_; } + static inline size_t dim() { return 3; }; + }; // Pose2 diff --git a/cpp/SubgraphPreconditioner-inl.h b/cpp/SubgraphPreconditioner-inl.h index d7928fd05..889b8006a 100644 --- a/cpp/SubgraphPreconditioner-inl.h +++ b/cpp/SubgraphPreconditioner-inl.h @@ -24,7 +24,7 @@ namespace gtsam { SubgraphPCG::SubgraphPCG(const G& g, const T& theta0) : maxIterations_(100), verbose_(false), epsilon_(1e-4), epsilon_abs_(1e-5) { - // generate spanning tree and create ordering + // generate spanning tree PredecessorMap tree = g.template findMinimumSpanningTree(); list keys = predecessorMap2Keys(tree); @@ -57,10 +57,10 @@ namespace gtsam { /* ************************************************************************* */ template - VectorConfig SubgraphPCG::optimize(SubgraphPreconditioner& system, const Ordering& ordering) const { + VectorConfig SubgraphPCG::optimize(SubgraphPreconditioner& system) const { //TODO: 3 is hard coded here VectorConfig zeros; - BOOST_FOREACH(const Symbol& j, ordering) zeros.insert(j,zero(3)); + BOOST_FOREACH(const Symbol& j, *ordering_) zeros.insert(j,zero(Pose::dim())); // Solve the subgraph PCG VectorConfig ybar = conjugateGradients ordering() const { return ordering_; } @@ -108,7 +105,7 @@ namespace gtsam { * solve for the optimal displacement in the tangent space, and then solve * the resulted linear system */ - VectorConfig optimize(SubgraphPreconditioner& system, const Ordering& ordering) const; + VectorConfig optimize(SubgraphPreconditioner& system) const; }; } // nsamespace gtsam diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index cdcb1f8ec..d763a9bea 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -35,152 +35,157 @@ using namespace example; typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ -//TEST( NonlinearOptimizer, delta ) -//{ -// shared_ptr fg(new Graph( -// createNonlinearFactorGraph())); -// Optimizer::shared_config initial = sharedNoisyConfig(); -// -// // Expected configuration is the difference between the noisy config -// // and the ground-truth config. One step only because it's linear ! -// VectorConfig expected; -// Vector dl1(2); -// dl1(0) = -0.1; -// dl1(1) = 0.1; -// expected.insert("l1", dl1); -// Vector dx1(2); -// dx1(0) = -0.1; -// dx1(1) = -0.1; -// expected.insert("x1", dx1); -// Vector dx2(2); -// dx2(0) = 0.1; -// dx2(1) = -0.2; -// expected.insert("x2", dx2); -// -// // Check one ordering -// 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 -// 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... -// shared_ptr ord3(new Ordering()); -// *ord3 += "l1","x1","x2"; -// Optimizer optimizer3(fg, ord3, initial); -// VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta(); -// CHECK(assert_equal(actual3,expected)); -//} -// -///* ************************************************************************* */ -//TEST( NonlinearOptimizer, iterateLM ) -//{ -// // really non-linear factor graph -// shared_ptr fg(new Graph( -// createReallyNonlinearFactorGraph())); -// -// // config far from minimum -// Point2 x0(3,0); -// boost::shared_ptr config(new Config); -// config->insert(simulated2D::PoseKey(1), x0); -// -// // ordering -// shared_ptr ord(new Ordering()); -// ord->push_back("x1"); -// -// // create initial optimization state, with lambda=0 -// Optimizer::shared_solver solver(new Factorization< -// Graph, Config> ); -// Optimizer optimizer(fg, ord, config, solver, 0.); -// -// // normal iterate -// Optimizer iterated1 = optimizer.iterate(); -// -// // LM iterate with lambda 0 should be the same -// Optimizer iterated2 = optimizer.iterateLM(); -// -// // Try successive iterates. TODO: ugly pointers, better way ? -// Optimizer *pointer = new Optimizer(iterated2); -// for (int i=0;i<10;i++) { -// Optimizer* newOptimizer = new Optimizer(pointer->iterateLM()); -// delete pointer; -// pointer = newOptimizer; -// } -// delete(pointer); -// -// CHECK(assert_equal(*iterated1.config(), *iterated2.config(), 1e-9)); -//} -// -///* ************************************************************************* */ -//TEST( NonlinearOptimizer, optimize ) -//{ -// shared_ptr fg(new Graph( -// createReallyNonlinearFactorGraph())); -// -// // test error at minimum -// Point2 xstar(0,0); -// Config cstar; -// cstar.insert(simulated2D::PoseKey(1), xstar); -// DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); -// -// // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = -// Point2 x0(3,3); -// boost::shared_ptr c0(new Config); -// c0->insert(simulated2D::PoseKey(1), x0); -// DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); -// -// // optimize parameters -// shared_ptr ord(new Ordering()); -// ord->push_back("x1"); -// double relativeThreshold = 1e-5; -// double absoluteThreshold = 1e-5; -// -// // initial optimization state is the same in both cases tested -// Optimizer optimizer(fg, ord, c0); -// -// // Gauss-Newton -// Optimizer actual1 = optimizer.gaussNewton(relativeThreshold, -// absoluteThreshold); -// DOUBLES_EQUAL(0,fg->error(*(actual1.config())),1e-3); -// -// // Levenberg-Marquardt -// Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold, -// absoluteThreshold, Optimizer::SILENT); -// DOUBLES_EQUAL(0,fg->error(*(actual2.config())),1e-3); -//} -// -///* ************************************************************************* */ -//TEST( NonlinearOptimizer, Factorization ) -//{ -// typedef NonlinearOptimizer > Optimizer; -// -// boost::shared_ptr config(new Pose2Config); -// config->insert(1, Pose2(0.,0.,0.)); -// config->insert(2, Pose2(1.5,0.,0.)); -// -// boost::shared_ptr graph(new Pose2Graph); -// graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); -// graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); -// -// boost::shared_ptr ordering(new Ordering); -// ordering->push_back(Pose2Config::Key(1)); -// ordering->push_back(Pose2Config::Key(2)); -// -// Optimizer optimizer(graph, ordering, config); -// Optimizer optimized = optimizer.iterateLM(); -// -// Pose2Config expected; -// expected.insert(1, Pose2(0.,0.,0.)); -// expected.insert(2, Pose2(1.,0.,0.)); -// CHECK(assert_equal(expected, *optimized.config(), 1e-5)); -//} +TEST( NonlinearOptimizer, delta ) +{ + shared_ptr fg(new Graph( + createNonlinearFactorGraph())); + Optimizer::shared_config initial = sharedNoisyConfig(); + + // Expected configuration is the difference between the noisy config + // and the ground-truth config. One step only because it's linear ! + VectorConfig expected; + Vector dl1(2); + dl1(0) = -0.1; + dl1(1) = 0.1; + expected.insert("l1", dl1); + Vector dx1(2); + dx1(0) = -0.1; + dx1(1) = -0.1; + expected.insert("x1", dx1); + Vector dx2(2); + dx2(0) = 0.1; + dx2(1) = -0.2; + expected.insert("x2", dx2); + + // Check one ordering + shared_ptr ord1(new Ordering()); + *ord1 += "x2","l1","x1"; + Optimizer::shared_solver solver; + solver = Optimizer::shared_solver(new Optimizer::solver(ord1)); + Optimizer optimizer1(fg, initial, solver); + VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta(); + CHECK(assert_equal(actual1,expected)); + + // Check another + shared_ptr ord2(new Ordering()); + *ord2 += "x1","x2","l1"; + solver = Optimizer::shared_solver(new Optimizer::solver(ord2)); + Optimizer optimizer2(fg, initial, solver); + VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta(); + CHECK(assert_equal(actual2,expected)); + + // And yet another... + shared_ptr ord3(new Ordering()); + *ord3 += "l1","x1","x2"; + solver = Optimizer::shared_solver(new Optimizer::solver(ord3)); + Optimizer optimizer3(fg, initial, solver); + VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta(); + CHECK(assert_equal(actual3,expected)); +} + +/* ************************************************************************* */ +TEST( NonlinearOptimizer, iterateLM ) +{ + // really non-linear factor graph + shared_ptr fg(new Graph( + createReallyNonlinearFactorGraph())); + + // config far from minimum + Point2 x0(3,0); + boost::shared_ptr config(new Config); + config->insert(simulated2D::PoseKey(1), x0); + + // ordering + shared_ptr ord(new Ordering()); + ord->push_back("x1"); + + // create initial optimization state, with lambda=0 + Optimizer::shared_solver solver(new Optimizer::solver(ord)); + Optimizer optimizer(fg, config, solver, 0.); + + // normal iterate + Optimizer iterated1 = optimizer.iterate(); + + // LM iterate with lambda 0 should be the same + Optimizer iterated2 = optimizer.iterateLM(); + + // Try successive iterates. TODO: ugly pointers, better way ? + Optimizer *pointer = new Optimizer(iterated2); + for (int i=0;i<10;i++) { + Optimizer* newOptimizer = new Optimizer(pointer->iterateLM()); + delete pointer; + pointer = newOptimizer; + } + delete(pointer); + + CHECK(assert_equal(*iterated1.config(), *iterated2.config(), 1e-9)); +} + +/* ************************************************************************* */ +TEST( NonlinearOptimizer, optimize ) +{ + shared_ptr fg(new Graph( + createReallyNonlinearFactorGraph())); + + // test error at minimum + Point2 xstar(0,0); + Config cstar; + cstar.insert(simulated2D::PoseKey(1), xstar); + DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); + + // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = + Point2 x0(3,3); + boost::shared_ptr c0(new Config); + c0->insert(simulated2D::PoseKey(1), x0); + DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); + + // optimize parameters + shared_ptr ord(new Ordering()); + ord->push_back("x1"); + double relativeThreshold = 1e-5; + double absoluteThreshold = 1e-5; + + // initial optimization state is the same in both cases tested + Optimizer::shared_solver solver(new Optimizer::solver(ord)); + Optimizer optimizer(fg, c0, solver); + + // Gauss-Newton + Optimizer actual1 = optimizer.gaussNewton(relativeThreshold, + absoluteThreshold); + DOUBLES_EQUAL(0,fg->error(*(actual1.config())),1e-3); + + // Levenberg-Marquardt + Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold, + absoluteThreshold, Optimizer::SILENT); + DOUBLES_EQUAL(0,fg->error(*(actual2.config())),1e-3); +} + +/* ************************************************************************* */ +TEST( NonlinearOptimizer, Factorization ) +{ + typedef NonlinearOptimizer > Optimizer; + + boost::shared_ptr config(new Pose2Config); + config->insert(1, Pose2(0.,0.,0.)); + config->insert(2, Pose2(1.5,0.,0.)); + + boost::shared_ptr graph(new Pose2Graph); + graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); + graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); + + boost::shared_ptr ordering(new Ordering); + ordering->push_back(Pose2Config::Key(1)); + ordering->push_back(Pose2Config::Key(2)); + Optimizer::shared_solver solver(new Factorization(ordering)); + + Optimizer optimizer(graph, config, solver); + Optimizer optimized = optimizer.iterateLM(); + + Pose2Config expected; + expected.insert(1, Pose2(0.,0.,0.)); + expected.insert(2, Pose2(1.,0.,0.)); + CHECK(assert_equal(expected, *optimized.config(), 1e-5)); +} /* ************************************************************************* */ TEST( NonlinearOptimizer, SubgraphPCG ) @@ -195,14 +200,10 @@ TEST( NonlinearOptimizer, SubgraphPCG ) graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); - boost::shared_ptr ordering(new Ordering); - ordering->push_back(Pose2Config::Key(1)); - ordering->push_back(Pose2Config::Key(2)); - double relativeThreshold = 1e-5; double absoluteThreshold = 1e-5; Optimizer::shared_solver solver(new SubgraphPCG(*graph, *config)); - Optimizer optimizer(graph, ordering, config, solver); + Optimizer optimizer(graph, config, solver); Optimizer optimized = optimizer.gaussNewton(relativeThreshold, absoluteThreshold, Optimizer::SILENT); Pose2Config expected; diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index f8f236a14..3b2b73dbd 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -101,7 +101,8 @@ TEST(Pose2Graph, optimize) { shared_ptr ordering(new Ordering); *ordering += "x0","x1"; typedef NonlinearOptimizer Optimizer; - Optimizer optimizer0(fg, ordering, initial); + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer0(fg, initial, solver); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; //Optimizer::verbosityLevel verbosity = Optimizer::ERROR; Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); @@ -145,7 +146,8 @@ TEST(Pose2Graph, optimizeCircle) { shared_ptr ordering(new Ordering); *ordering += "x0","x1","x2","x3","x4","x5"; typedef NonlinearOptimizer Optimizer; - Optimizer optimizer0(fg, ordering, initial); + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer0(fg, initial, solver); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; // Optimizer::verbosityLevel verbosity = Optimizer::ERROR; Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); diff --git a/cpp/testPose3SLAM.cpp b/cpp/testPose3SLAM.cpp index 5920574fc..6d8eeb411 100644 --- a/cpp/testPose3SLAM.cpp +++ b/cpp/testPose3SLAM.cpp @@ -58,7 +58,8 @@ TEST(Pose3Graph, optimizeCircle) { shared_ptr ordering(new Ordering); *ordering += "x0","x1","x2","x3","x4","x5"; typedef NonlinearOptimizer Optimizer; - Optimizer optimizer0(fg, ordering, initial); + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer0(fg, initial, solver); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; // Optimizer::verbosityLevel verbosity = Optimizer::ERROR; Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 29da30308..27c0e0d13 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -296,7 +296,8 @@ TEST (SQP, two_pose_truth ) { // optimize the graph shared_ptr ordering(new Ordering()); *ordering += "x1", "x2", "l1"; - Optimizer optimizer(graph, ordering, initialEstimate); + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer(graph, initialEstimate, solver); // display solution double relativeThreshold = 1e-5; @@ -533,7 +534,8 @@ TEST (SQP, stereo_truth ) { *ord += "x1", "x2", "l1"; // create optimizer - VOptimizer optimizer(graph, ord, truthConfig); + VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); + VOptimizer optimizer(graph, truthConfig, solver); // optimize VOptimizer afterOneIteration = optimizer.iterate(); @@ -609,7 +611,8 @@ TEST (SQP, stereo_truth_noisy ) { *ord += "x1", "x2", "l1"; // create optimizer - VOptimizer optimizer0(graph, ord, noisyConfig); + VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); + VOptimizer optimizer0(graph, noisyConfig, solver); if (verbose) cout << "Initial Error: " << optimizer0.error() << endl; diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 818adfcbb..9a3b36839 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -100,7 +100,8 @@ TEST( Graph, optimizeLM) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - Optimizer optimizer(graph, ordering, initialEstimate); + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer(graph, initialEstimate, solver); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started @@ -144,7 +145,8 @@ TEST( Graph, optimizeLM2) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - Optimizer optimizer(graph, ordering, initialEstimate); + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer(graph, initialEstimate, solver); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started @@ -179,7 +181,8 @@ TEST( Graph, CHECK_ORDERING) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - Optimizer optimizer(graph, ordering, initialEstimate); + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer(graph, initialEstimate, solver); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started