From 829bb1f8aa4c917ed31459c3205233f61eae3298 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 23 Mar 2012 22:43:59 +0000 Subject: [PATCH] Added 'optimized' shortcut function to optimize and return Values directly --- gtsam/nonlinear/NonlinearOptimizer.h | 12 ++++++++++++ gtsam/slam/planarSLAM.h | 2 +- gtsam/slam/pose2SLAM.h | 2 +- gtsam/slam/tests/testPose3SLAM.cpp | 6 +++--- tests/testBoundingConstraint.cpp | 4 ++-- tests/testNonlinearEquality.cpp | 12 ++++++------ tests/testNonlinearOptimizer.cpp | 10 +++++----- tests/testRot3Optimization.cpp | 2 +- 8 files changed, 31 insertions(+), 19 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 61edaf754..4dd81c8d9 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -124,9 +124,21 @@ public: /** Optimize for the maximum-likelihood estimate, returning a new * NonlinearOptimizer class containing the optimized variable assignments, * which may be retrieved with values(). + * + * This function simply calls iterate() in a loop, checking for convergence + * with check_convergence(). For fine-grain control over the optimization + * process, you may call iterate() and check_convergence() yourself, and if + * needed modify the optimization state between iterations. */ virtual auto_ptr optimize() const { return defaultOptimize(); } + /** Shortcut to optimize and return the resulting Values of the maximum- + * likelihood estimate. To access statistics and information such as the + * final error and number of iterations, use optimize() instead. + * @return The maximum-likelihood estimate. + */ + virtual SharedValues optimized() const { return this->optimize()->values(); } + /** Retrieve the current variable assignment estimate. */ virtual const SharedValues& values() const { return values_; } diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index bd7de3a91..49463a4db 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, initialEstimate).optimize()->values(); + return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimized(); } }; diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 6e448b4ee..7fb86a46a 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -97,7 +97,7 @@ namespace pose2SLAM { /// Optimize Values optimize(const Values& initialEstimate) { - return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimize()->values(); + return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimized(); } }; diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 059e5b762..2c9562275 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, initial, ordering).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(fg, initial, ordering).optimized(); // 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, values).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, values).optimized(); 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, values).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, values).optimized(); EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 32495f4e7..c062484a6 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, initValues).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized(); 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, initValues).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized(); Values expected; expected.insert(x1, goal_pt); CHECK(assert_equal(expected, actual, tol)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 39051b8c4..ce86e71bc 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -230,7 +230,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // optimize Ordering ordering; ordering.push_back(key1); - Values actual = *LevenbergMarquardtOptimizer(graph, init, ordering).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, init, ordering).optimized(); // 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).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized(); 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).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized(); 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).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized(); 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).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized(); 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).optimize()->values(); + Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized(); // create config Values truthValues; diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index cb97a6ae7..4e35c0f77 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -115,7 +115,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer ) boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); - Values::const_shared_ptr actual = LevenbergMarquardtOptimizer(fg, c0).optimize()->values(); + Values::const_shared_ptr actual = LevenbergMarquardtOptimizer(fg, c0).optimized(); DOUBLES_EQUAL(0,fg->error(*actual),tol); } @@ -129,7 +129,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer ) boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); - Values::const_shared_ptr actual = GaussNewtonOptimizer(fg, c0).optimize()->values(); + Values::const_shared_ptr actual = GaussNewtonOptimizer(fg, c0).optimized(); DOUBLES_EQUAL(0,fg->error(*actual),tol); } @@ -143,7 +143,7 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer ) boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); - Values::const_shared_ptr actual = DoglegOptimizer(fg, c0).optimize()->values(); + Values::const_shared_ptr actual = DoglegOptimizer(fg, c0).optimized(); DOUBLES_EQUAL(0,fg->error(*actual),tol); } @@ -161,10 +161,10 @@ TEST( NonlinearOptimizer, optimization_method ) Values c0; c0.insert(simulated2D::PoseKey(1), x0); - Values actualMFQR = *LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize()->values(); + Values actualMFQR = *LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimized(); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); - Values actualMFLDL = *LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimize()->values(); + Values actualMFLDL = *LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimized(); DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol); } diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index ec7f1ddfa..1c90f4f48 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).optimize()->values(); + Values final = *GaussNewtonOptimizer(fg, initial).optimized(); EXPECT(assert_equal(truth, final, 1e-5)); }