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

fixed unit test

release/4.3a0
Luca 2014-02-16 17:11:58 -05:00
parent 4aec076568
commit c05eaa0d22
2 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -129,7 +129,7 @@ void LevenbergMarquardtOptimizer::iterate() {
A.diagonal() = hessianDiagonal.at(key_value.key);
for (size_t aa=0; aa<dim; aa++)
{
A(aa,aa)=sqrt(std::min(std::max(A(aa,aa), min_diagonal_), max_diagonal_));
A(aa,aa)= sqrt(std::min(std::max(A(aa,aa), min_diagonal_), max_diagonal_));
}
}
Vector b = Vector::Zero(dim);

11
tests/testNonlinearOptimizer.cpp
View File

@ -256,13 +256,14 @@ TEST(NonlinearOptimizer, MoreOptimization) {
EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
// Try LM with diagonal damping
Values initBetter;
initBetter.insert(0, Pose2(3,4,0));
initBetter.insert(1, Pose2(10,2,M_PI/3));
initBetter.insert(2, Pose2(11,7,M_PI/2));
LevenbergMarquardtParams params;
params.verbosityLM = gtsam::LevenbergMarquardtParams::DAMPED;
params.verbosity = gtsam::NonlinearOptimizerParams::ERROR;
params.setDiagonalDamping(true);
params.setRelativeErrorTol(1e-9);
params.setAbsoluteErrorTol(1e-9);
LevenbergMarquardtOptimizer optimizer(fg, init, params);
LevenbergMarquardtOptimizer optimizer(fg, initBetter, params);
EXPECT(assert_equal(expected, optimizer.optimize()));
}