12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added 'optimized' shortcut function to optimize and return Values directly

release/4.3a0
Richard Roberts 2012-03-23 22:43:59 +00:00
parent cdd89a43f5
commit 829bb1f8aa
8 changed files with 31 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -124,9 +124,21 @@ public:
/** Optimize for the maximum-likelihood estimate, returning a new /** Optimize for the maximum-likelihood estimate, returning a new
* NonlinearOptimizer class containing the optimized variable assignments, * NonlinearOptimizer class containing the optimized variable assignments,
* which may be retrieved with values(). * 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(); } 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. */ /** Retrieve the current variable assignment estimate. */
virtual const SharedValues& values() const { return values_; } virtual const SharedValues& values() const { return values_; }

2
gtsam/slam/planarSLAM.h
View File

@ -110,7 +110,7 @@ namespace planarSLAM {
/// Optimize /// Optimize
Values optimize(const Values& initialEstimate) { Values optimize(const Values& initialEstimate) {
return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimize()->values(); return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimized();
} }
}; };

2
gtsam/slam/pose2SLAM.h
View File

@ -97,7 +97,7 @@ namespace pose2SLAM {
/// Optimize /// Optimize
Values optimize(const Values& initialEstimate) { Values optimize(const Values& initialEstimate) {
return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimize()->values(); return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimized();
} }
}; };

6
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -78,7 +78,7 @@ TEST(Pose3Graph, optimizeCircle) {
Ordering ordering; Ordering ordering;
ordering += kx0,kx1,kx2,kx3,kx4,kx5; 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 with ground truth
CHECK(assert_equal(hexagon, actual,1e-4)); CHECK(assert_equal(hexagon, actual,1e-4));
@ -113,7 +113,7 @@ TEST(Pose3Graph, partial_prior_height) {
// linearization // linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); 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<Pose3>(key), tol)); EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
} }
@ -165,7 +165,7 @@ TEST(Pose3Graph, partial_prior_xy) {
Values values; Values values;
values.insert(key, init); values.insert(key, init);
Values actual = *LevenbergMarquardtOptimizer(graph, values).optimize()->values(); Values actual = *LevenbergMarquardtOptimizer(graph, values).optimized();
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol)); EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
} }

4
tests/testBoundingConstraint.cpp
View File

@ -154,7 +154,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
Values initValues; Values initValues;
initValues.insert(x1, start_pt); initValues.insert(x1, start_pt);
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values(); Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized();
Values expected; Values expected;
expected.insert(x1, goal_pt); expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, actual, tol)); CHECK(assert_equal(expected, actual, tol));
@ -176,7 +176,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
Values initValues; Values initValues;
initValues.insert(x1, start_pt); initValues.insert(x1, start_pt);
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values(); Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized();
Values expected; Values expected;
expected.insert(x1, goal_pt); expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, actual, tol)); CHECK(assert_equal(expected, actual, tol));

12
tests/testNonlinearEquality.cpp
View File

@ -230,7 +230,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// optimize // optimize
Ordering ordering; Ordering ordering;
ordering.push_back(key1); ordering.push_back(key1);
Values actual = *LevenbergMarquardtOptimizer(graph, init, ordering).optimize()->values(); Values actual = *LevenbergMarquardtOptimizer(graph, init, ordering).optimized();
// verify // verify
Values expected; Values expected;
@ -317,7 +317,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
EXPECT(constraint->active(expected)); EXPECT(constraint->active(expected));
EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); 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)); EXPECT(assert_equal(expected, actual, tol));
} }
@ -408,7 +408,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
initValues.insert(key1, Point2()); initValues.insert(key1, Point2());
initValues.insert(key2, badPt); initValues.insert(key2, badPt);
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values(); Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized();
Values expected; Values expected;
expected.insert(key1, truth_pt1); expected.insert(key1, truth_pt1);
expected.insert(key2, truth_pt2); 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(l1, Point2(1.0, 6.0)); // ground truth
initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame 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; Values expected;
expected.insert(x1, pt_x1); 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 initialEstimate.insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin
// optimize // optimize
Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimize()->values(); Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized();
Values expected; Values expected;
expected.insert(x1, Point2(1.0, 1.0)); expected.insert(x1, Point2(1.0, 1.0));
@ -557,7 +557,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
initValues.insert(l2, landmark2); initValues.insert(l2, landmark2);
// optimize // optimize
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values(); Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized();
// create config // create config
Values truthValues; Values truthValues;

10
tests/testNonlinearOptimizer.cpp
View File

@ -115,7 +115,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer )
boost::shared_ptr<Values> c0(new Values); boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0); 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); DOUBLES_EQUAL(0,fg->error(*actual),tol);
} }
@ -129,7 +129,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer )
boost::shared_ptr<Values> c0(new Values); boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0); 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); DOUBLES_EQUAL(0,fg->error(*actual),tol);
} }
@ -143,7 +143,7 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer )
boost::shared_ptr<Values> c0(new Values); boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0); 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); DOUBLES_EQUAL(0,fg->error(*actual),tol);
} }
@ -161,10 +161,10 @@ TEST( NonlinearOptimizer, optimization_method )
Values c0; Values c0;
c0.insert(simulated2D::PoseKey(1), x0); 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); 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); DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
} }

2
tests/testRot3Optimization.cpp
View File

@ -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))); 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)); EXPECT(assert_equal(truth, final, 1e-5));
} }