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
* 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_; }

2
gtsam/slam/planarSLAM.h
View File

@ -110,7 +110,7 @@ namespace planarSLAM {
/// Optimize
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
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 += 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<Pose3>(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<Pose3>(key), 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;
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));

12
tests/testNonlinearEquality.cpp
View File

@ -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;

10
tests/testNonlinearOptimizer.cpp
View File

@ -115,7 +115,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer )
boost::shared_ptr<Values> 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<Values> 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<Values> 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);
}

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)));
}
Values final = *GaussNewtonOptimizer(fg, initial).optimize()->values();
Values final = *GaussNewtonOptimizer(fg, initial).optimized();
EXPECT(assert_equal(truth, final, 1e-5));
}