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() {} virtual ~DoglegState() {}
protected: 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) {} NonlinearOptimizerState(graph, values, iterations), Delta(params.deltaInitial) {}
friend class DoglegOptimizer; friend class DoglegOptimizer;
@ -105,7 +105,7 @@ public:
*/ */
DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) : DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
NonlinearOptimizer(graph) { NonlinearOptimizer(graph) {
*params_.ordering = ordering; params_.ordering = ordering;
state_ = DoglegState(graph, initialValues, params_); } 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) : GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
NonlinearOptimizer(graph), state_(graph, initialValues) { NonlinearOptimizer(graph), state_(graph, initialValues) {
*params_.ordering = ordering; } params_.ordering = ordering; }
/// @} /// @}

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

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

23
tests/testNonlinearOptimizer.cpp
View File

@ -54,11 +54,14 @@ TEST( NonlinearOptimizer, iterateLM )
config.insert(simulated2D::PoseKey(1), x0); config.insert(simulated2D::PoseKey(1), x0);
// normal iterate // normal iterate
GaussNewtonOptimizer gnOptimizer(fg, config); GaussNewtonParams gnParams;
GaussNewtonOptimizer gnOptimizer(fg, config, gnParams);
gnOptimizer.iterate(); gnOptimizer.iterate();
// LM iterate with lambda 0 should be the same // 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(); lmOptimizer.iterate();
CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9)); CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9));
@ -81,16 +84,26 @@ TEST( NonlinearOptimizer, optimize )
c0.insert(simulated2D::PoseKey(1), x0); c0.insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters
Ordering ord;
ord.push_back(kx(1));
// Gauss-Newton // 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); DOUBLES_EQUAL(0,fg.error(actual1),tol);
// Levenberg-Marquardt // 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); DOUBLES_EQUAL(0,fg.error(actual2),tol);
// Dogleg // 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); DOUBLES_EQUAL(0,fg.error(actual3),tol);
} }