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

Fixed bug with shared solvers in recursive LM nonlinear optimizer

release/4.3a0
Alex Cunningham 2011-02-10 16:01:28 +00:00
parent 7404f78bc1
commit 06b08c6f85
2 changed files with 53 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -292,6 +292,7 @@ namespace gtsam {
double next_error = error_;
shared_values next_values = values_;
shared_solver solver = solver_;
while(true) {
if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl;
@ -314,9 +315,9 @@ namespace gtsam {
if (verbosity >= Parameters::DAMPED) damped->print("damped");
// solve
S solver(*damped); // not solver_ !!
solver.reset(new S(*damped));
VectorValues delta = *solver.optimize();
VectorValues delta = *solver->optimize();
if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
// update values
@ -332,8 +333,7 @@ namespace gtsam {
next_error = error;
lambda /= factor;
break;
}
else {
} else {
// Either we're not cautious, or the same lambda was worse than the current error.
// The more adventurous lambda was worse too, so make lambda more conservative
// and keep the same values.
@ -345,7 +345,8 @@ namespace gtsam {
}
} // end while
return newValuesErrorLambda_(next_values, next_error, lambda);
return NonlinearOptimizer(graph_, next_values, next_error, ordering_, solver,
parameters_->newLambda_(lambda), dimensions_, iterations_);
}
/* ************************************************************************* */

12
tests/testNonlinearOptimizer.cpp
View File

@ -187,13 +187,13 @@ TEST( NonlinearOptimizer, optimize_LM_recursive )
Point2 xstar(0,0);
example::Values cstar;
cstar.insert(simulated2D::PoseKey(1), xstar);
DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
EXPECT_DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3);
boost::shared_ptr<example::Values> c0(new example::Values);
c0->insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
EXPECT_DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
// optimize parameters
shared_ptr<Ordering> ord(new Ordering());
@ -207,7 +207,13 @@ TEST( NonlinearOptimizer, optimize_LM_recursive )
// Levenberg-Marquardt
Optimizer actual2 = optimizer.levenbergMarquardtRecursive();
DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol);
EXPECT_DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol);
// calculate the marginal
Matrix actualCovariance; Vector mean;
boost::tie(mean, actualCovariance) = actual2.marginalCovariance("x1");
Matrix expectedCovariance = Matrix_(2,2, 8.60817108, 0.0, 0.0, 0.01);
EXPECT(assert_equal(expectedCovariance, actualCovariance, tol));
}
/* ************************************************************************* */