diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 887f6b86c..88f4dc65e 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -175,8 +175,9 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // Create an ordering of the variables Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * 1e-5 * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); } /* ************************************************************************* */ @@ -217,8 +218,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double reproj_error = 1e-5; Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * reproj_error * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -257,8 +259,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const double reproj_error = 1e-5 ; Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * reproj_error * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -313,8 +316,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const double reproj_error = 1e-5 ; Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * reproj_error * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -356,8 +360,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double reproj_error = 1e-5 ; Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * reproj_error * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -380,7 +385,7 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { LevenbergMarquardtParams params; params.absoluteErrorTol = 1e-9; params.relativeErrorTol = 1e-9; - Values actual = LevenbergMarquardtOptimizer(graph, params).optimized(init); + Values actual = LevenbergMarquardtOptimizer(graph, init, params).optimize(); EXPECT(assert_equal(expected, actual, 1e-4)); } @@ -404,7 +409,7 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { LevenbergMarquardtParams params; params.absoluteErrorTol = 1e-9; params.relativeErrorTol = 1e-9; - Values actual = LevenbergMarquardtOptimizer(graph, params).optimized(init); + Values actual = LevenbergMarquardtOptimizer(graph, init, params).optimize(); EXPECT(assert_equal(expected, actual, 1e-4)); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index ed7c35663..8a6c3b575 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -176,8 +176,9 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // Create an ordering of the variables Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * 1e-5 * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); } /* ************************************************************************* */ @@ -218,8 +219,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double reproj_error = 1e-5; Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * reproj_error * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -258,8 +260,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const double reproj_error = 1e-5 ; Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * reproj_error * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -310,8 +313,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const double reproj_error = 1e-5 ; Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * reproj_error * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ @@ -353,8 +357,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double reproj_error = 1e-5 ; Ordering ordering = *getOrdering(X,L); - NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values); - EXPECT(final->error < 0.5 * reproj_error * nMeasurements); + LevenbergMarquardtOptimizer optimizer(graph, values, ordering); + Values final = optimizer.optimize(); + EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 5555b03b3..22c02ab52 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -164,7 +164,7 @@ TEST(Pose2Graph, optimize) { LevenbergMarquardtParams params; params.relativeErrorTol = 1e-15; params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial); + Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); // Check with expected config Values expected; @@ -203,7 +203,7 @@ TEST(Pose2Graph, optimizeThreePoses) { LevenbergMarquardtParams params; params.relativeErrorTol = 1e-15; params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial); + Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); // Check with ground truth CHECK(assert_equal((const Values&)hexagon, actual)); @@ -245,7 +245,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { LevenbergMarquardtParams params; params.relativeErrorTol = 1e-15; params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial); + Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); // Check with ground truth CHECK(assert_equal((const Values&)hexagon, actual)); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index dac562267..688dcc7b9 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -78,7 +78,7 @@ TEST(Pose3Graph, optimizeCircle) { Ordering ordering; ordering += kx0,kx1,kx2,kx3,kx4,kx5; - Values actual = LevenbergMarquardtOptimizer(fg, ordering).optimized(initial); + Values actual = LevenbergMarquardtOptimizer(fg, initial, ordering).optimize(); // Check with ground truth CHECK(assert_equal(hexagon, actual,1e-4)); @@ -113,7 +113,7 @@ TEST(Pose3Graph, partial_prior_height) { // linearization EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - Values actual = LevenbergMarquardtOptimizer(graph).optimized(values); + Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -165,7 +165,7 @@ TEST(Pose3Graph, partial_prior_xy) { Values values; values.insert(key, init); - Values actual = LevenbergMarquardtOptimizer(graph).optimized(values); + Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index bfc93fbaa..25cd6e16f 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -65,20 +65,19 @@ TEST( StereoFactor, singlePoint) Point3 l1(0, 0, 0); values.insert(PointKey(1), l1); // add point at origin; - GaussNewtonOptimizer optimizer(graph); - NonlinearOptimizer::SharedState initial = optimizer.initialState(values); + GaussNewtonOptimizer optimizer(graph, values); // We expect the initial to be zero because config is the ground truth - DOUBLES_EQUAL(0.0, initial->error, 1e-9); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed - NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial); - DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9); + optimizer.iterate(); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Complete solution - NonlinearOptimizer::SharedState final = optimizer.optimize(initial); + optimizer.optimize(); - DOUBLES_EQUAL(0.0, final->error, 1e-6); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-6); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index 0c42772de..bb7623954 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -102,17 +102,16 @@ TEST( Graph, optimizeLM) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - const NonlinearOptimizer& optimizer = LevenbergMarquardtOptimizer(graph, ordering); - NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate); - DOUBLES_EQUAL(0.0, initial->error, 1e-9); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial); - DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9); + optimizer.iterate(); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct - CHECK(assert_equal(initialEstimate, afterOneIteration->values)); + CHECK(assert_equal(initialEstimate, optimizer.values())); } @@ -140,17 +139,16 @@ TEST( Graph, optimizeLM2) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, ordering); - NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate); - DOUBLES_EQUAL(0.0, initial->error, 1e-9); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial); - DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9); + optimizer.iterate(); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct - CHECK(assert_equal(initialEstimate, afterOneIteration->values)); + CHECK(assert_equal(initialEstimate, optimizer.values())); } @@ -174,17 +172,16 @@ TEST( Graph, CHECK_ORDERING) // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph); - NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate); - DOUBLES_EQUAL(0.0, initial->error, 1e-9); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // Iterate once, and the config should not have changed because we started // with the ground truth - NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial); - DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9); + optimizer.iterate(); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); // check if correct - CHECK(assert_equal(initialEstimate, afterOneIteration->values)); + CHECK(assert_equal(initialEstimate, optimizer.values())); } /* ************************************************************************* */ diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index b9230705a..e1b685fc8 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -154,7 +154,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { Values initValues; initValues.insert(x1, start_pt); - Values actual = LevenbergMarquardtOptimizer(graph).optimized(initValues); + Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); Values expected; expected.insert(x1, goal_pt); CHECK(assert_equal(expected, actual, tol)); @@ -176,7 +176,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { Values initValues; initValues.insert(x1, start_pt); - Values actual = LevenbergMarquardtOptimizer(graph).optimized(initValues); + Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); Values expected; expected.insert(x1, goal_pt); CHECK(assert_equal(expected, actual, tol)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index ce86e71bc..94ab58831 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -196,12 +196,12 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // optimize Ordering ordering; ordering.push_back(key1); - NonlinearOptimizer::auto_ptr result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize(); + Values result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize(); // verify Values expected; expected.insert(key1, feasible1); - EXPECT(assert_equal(expected, *result->values())); + EXPECT(assert_equal(expected, result)); } /* ************************************************************************* */ @@ -230,7 +230,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // optimize Ordering ordering; ordering.push_back(key1); - Values actual = *LevenbergMarquardtOptimizer(graph, init, ordering).optimized(); + Values actual = LevenbergMarquardtOptimizer(graph, init, ordering).optimize(); // verify Values expected; @@ -317,7 +317,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { EXPECT(constraint->active(expected)); EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); - Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized(); + Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); EXPECT(assert_equal(expected, actual, tol)); } @@ -408,7 +408,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { initValues.insert(key1, Point2()); initValues.insert(key2, badPt); - Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized(); + Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); Values expected; expected.insert(key1, truth_pt1); expected.insert(key2, truth_pt2); @@ -447,7 +447,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized(); + Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); Values expected; expected.insert(x1, pt_x1); @@ -491,7 +491,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { initialEstimate.insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin // optimize - Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized(); + Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); Values expected; expected.insert(x1, Point2(1.0, 1.0)); @@ -557,7 +557,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { initValues.insert(l2, landmark2); // optimize - Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized(); + Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); // create config Values truthValues; diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 4e35c0f77..e1088683e 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -46,105 +46,91 @@ Key kl(size_t i) { return Symbol('l',i); } TEST( NonlinearOptimizer, iterateLM ) { // really non-linear factor graph - shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); + example::Graph fg(example::createReallyNonlinearFactorGraph()); // config far from minimum Point2 x0(3,0); - boost::shared_ptr config(new Values); - config->insert(simulated2D::PoseKey(1), x0); - - // ordering - shared_ptr ord(new Ordering()); - ord->push_back(kx(1)); - - // create initial optimization state, with lambda=0 - NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0); + Values config; + config.insert(simulated2D::PoseKey(1), x0); // normal iterate - NonlinearOptimizer::auto_ptr iterated1 = GaussNewtonOptimizer(fg, config, GaussNewtonParams(), ord).iterate(); + GaussNewtonOptimizer gnOptimizer(fg, config); + gnOptimizer.iterate(); // LM iterate with lambda 0 should be the same - NonlinearOptimizer::auto_ptr iterated2 = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0)->iterate(); + LevenbergMarquardtOptimizer lmOptimizer(fg, config); + lmOptimizer.iterate(); - CHECK(assert_equal(*iterated1->values(), *iterated2->values(), 1e-9)); + CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9)); } /* ************************************************************************* */ TEST( NonlinearOptimizer, optimize ) { - shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); + example::Graph fg(example::createReallyNonlinearFactorGraph()); // test error at minimum Point2 xstar(0,0); Values cstar; cstar.insert(simulated2D::PoseKey(1), 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 = Point2 x0(3,3); - boost::shared_ptr c0(new Values); - 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(kx(1)); + Values c0; + c0.insert(simulated2D::PoseKey(1), x0); + DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); // Gauss-Newton - NonlinearOptimizer::auto_ptr actual1 = GaussNewtonOptimizer(fg, c0, GaussNewtonParams(), ord).optimize(); - DOUBLES_EQUAL(0,fg->error(*(actual1->values())),tol); + Values actual1 = GaussNewtonOptimizer(fg, c0).optimize(); + DOUBLES_EQUAL(0,fg.error(actual1),tol); // Levenberg-Marquardt - NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimize(); - DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol); + Values actual2 = LevenbergMarquardtOptimizer(fg, c0).optimize(); + DOUBLES_EQUAL(0,fg.error(actual2),tol); // Dogleg - NonlinearOptimizer::auto_ptr actual3 = DoglegOptimizer(fg, c0, DoglegParams(), ord).optimize(); - DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol); + Values actual3 = DoglegOptimizer(fg, c0).optimize(); + DOUBLES_EQUAL(0,fg.error(actual3),tol); } /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleLMOptimizer ) { - shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); + example::Graph fg(example::createReallyNonlinearFactorGraph()); Point2 x0(3,3); - boost::shared_ptr c0(new Values); - c0->insert(simulated2D::PoseKey(1), x0); + Values c0; + c0.insert(simulated2D::PoseKey(1), x0); - Values::const_shared_ptr actual = LevenbergMarquardtOptimizer(fg, c0).optimized(); - DOUBLES_EQUAL(0,fg->error(*actual),tol); + Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize(); + DOUBLES_EQUAL(0,fg.error(actual),tol); } /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleGNOptimizer ) { - shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); + example::Graph fg(example::createReallyNonlinearFactorGraph()); - Point2 x0(3,3); - boost::shared_ptr c0(new Values); - c0->insert(simulated2D::PoseKey(1), x0); + Point2 x0(3,3); + Values c0; + c0.insert(simulated2D::PoseKey(1), x0); - Values::const_shared_ptr actual = GaussNewtonOptimizer(fg, c0).optimized(); - DOUBLES_EQUAL(0,fg->error(*actual),tol); + Values actual = GaussNewtonOptimizer(fg, c0).optimize(); + DOUBLES_EQUAL(0,fg.error(actual),tol); } /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleDLOptimizer ) { - shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); + example::Graph fg(example::createReallyNonlinearFactorGraph()); Point2 x0(3,3); - boost::shared_ptr c0(new Values); - c0->insert(simulated2D::PoseKey(1), x0); + Values c0; + c0.insert(simulated2D::PoseKey(1), x0); - Values::const_shared_ptr actual = DoglegOptimizer(fg, c0).optimized(); - DOUBLES_EQUAL(0,fg->error(*actual),tol); + Values actual = DoglegOptimizer(fg, c0).optimize(); + DOUBLES_EQUAL(0,fg.error(actual),tol); } /* ************************************************************************* */ @@ -161,10 +147,10 @@ TEST( NonlinearOptimizer, optimization_method ) Values c0; c0.insert(simulated2D::PoseKey(1), x0); - Values actualMFQR = *LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimized(); + Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize(); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); - Values actualMFLDL = *LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimized(); + Values actualMFLDL = LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimize(); DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol); } @@ -183,12 +169,13 @@ TEST( NonlinearOptimizer, Factorization ) ordering.push_back(pose2SLAM::PoseKey(1)); ordering.push_back(pose2SLAM::PoseKey(2)); - NonlinearOptimizer::auto_ptr optimized = LevenbergMarquardtOptimizer(graph, config, ordering).iterate(); + LevenbergMarquardtOptimizer optimizer(graph, config, ordering); + optimizer.iterate(); Values expected; expected.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.)); expected.insert(pose2SLAM::PoseKey(2), Pose2(1.,0.,0.)); - CHECK(assert_equal(expected, *optimized->values(), 1e-5)); + CHECK(assert_equal(expected, optimizer.values(), 1e-5)); } /* ************************************************************************* */ @@ -216,16 +203,16 @@ TEST(NonlinearOptimizer, NullFactor) { ord.push_back(kx(1)); // Gauss-Newton - NonlinearOptimizer::auto_ptr actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize(); - DOUBLES_EQUAL(0,fg.error(*actual1->values()),tol); + Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize(); + DOUBLES_EQUAL(0,fg.error(actual1),tol); // Levenberg-Marquardt - NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize(); - DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol); + Values actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize(); + DOUBLES_EQUAL(0,fg.error(actual2),tol); // Dogleg - NonlinearOptimizer::auto_ptr actual3 = DoglegOptimizer(fg, c0, ord).optimize(); - DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol); + Values actual3 = DoglegOptimizer(fg, c0, ord).optimize(); + DOUBLES_EQUAL(0,fg.error(actual3),tol); } /* ************************************************************************* */ diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index 1c90f4f48..df2471933 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -47,7 +47,7 @@ TEST(Rot3, optimize) { fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); } - Values final = *GaussNewtonOptimizer(fg, initial).optimized(); + Values final = GaussNewtonOptimizer(fg, initial).optimize(); EXPECT(assert_equal(truth, final, 1e-5)); }