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

Fixed errors in unit tests from updated NonlinearOptimizers

release/4.3a0
Stephen Williams 2012-05-14 22:31:42 +00:00
parent 93d1defc07
commit 754e289737
4 changed files with 23 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/nonlinear/DoglegOptimizer.h
View File

@ -65,7 +65,7 @@ public:
virtual ~DoglegState() {}
protected:
DoglegState(const NonlinearFactorGraph& graph, const Values& values, const DoglegParams& params, unsigned int interations = 0) :
DoglegState(const NonlinearFactorGraph& graph, const Values& values, const DoglegParams& params, unsigned int iterations = 0) :
NonlinearOptimizerState(graph, values, iterations), Delta(params.deltaInitial) {}
friend class DoglegOptimizer;
@ -105,7 +105,7 @@ public:
*/
DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
NonlinearOptimizer(graph) {
*params_.ordering = ordering;
params_.ordering = ordering;
state_ = DoglegState(graph, initialValues, params_); }
/// @}

2
gtsam/nonlinear/GaussNewtonOptimizer.h
View File

@ -70,7 +70,7 @@ public:
*/
GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
NonlinearOptimizer(graph), state_(graph, initialValues) {
*params_.ordering = ordering; }
params_.ordering = ordering; }
/// @}

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -74,7 +74,7 @@ public:
protected:
LevenbergMarquardtState(const NonlinearFactorGraph& graph, const Values& initialValues, const LevenbergMarquardtParams& params, unsigned int iterations = 0) :
NonlinearOptimizerState(graph, values, iterations), lambda(params.lambdaInitial) {}
NonlinearOptimizerState(graph, initialValues, iterations), lambda(params.lambdaInitial) {}
friend class LevenbergMarquardtOptimizer;
};
@ -114,7 +114,7 @@ public:
*/
LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
NonlinearOptimizer(graph), dimensions_(initialValues.dims(ordering)) {
*params_.ordering = ordering;
params_.ordering = ordering;
state_ = LevenbergMarquardtState(graph, initialValues, params_); }
/** Access the current damping value */

23
tests/testNonlinearOptimizer.cpp
View File

@ -54,11 +54,14 @@ TEST( NonlinearOptimizer, iterateLM )
config.insert(simulated2D::PoseKey(1), x0);
// normal iterate
GaussNewtonOptimizer gnOptimizer(fg, config);
GaussNewtonParams gnParams;
GaussNewtonOptimizer gnOptimizer(fg, config, gnParams);
gnOptimizer.iterate();
// LM iterate with lambda 0 should be the same
LevenbergMarquardtOptimizer lmOptimizer(fg, config);
LevenbergMarquardtParams lmParams;
lmParams.lambdaInitial = 0.0;
LevenbergMarquardtOptimizer lmOptimizer(fg, config, lmParams);
lmOptimizer.iterate();
CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9));
@ -81,16 +84,26 @@ TEST( NonlinearOptimizer, optimize )
c0.insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters
Ordering ord;
ord.push_back(kx(1));
// Gauss-Newton
Values actual1 = GaussNewtonOptimizer(fg, c0).optimize();
GaussNewtonParams gnParams;
gnParams.ordering = ord;
Values actual1 = GaussNewtonOptimizer(fg, c0, gnParams).optimize();
DOUBLES_EQUAL(0,fg.error(actual1),tol);
// Levenberg-Marquardt
Values actual2 = LevenbergMarquardtOptimizer(fg, c0).optimize();
LevenbergMarquardtParams lmParams;
lmParams.ordering = ord;
Values actual2 = LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
DOUBLES_EQUAL(0,fg.error(actual2),tol);
// Dogleg
Values actual3 = DoglegOptimizer(fg, c0).optimize();
DoglegParams dlParams;
dlParams.ordering = ord;
Values actual3 = DoglegOptimizer(fg, c0, dlParams).optimize();
DOUBLES_EQUAL(0,fg.error(actual3),tol);
}