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

Added "SEARCH_REDUCE_ONLY" mode to Dogleg

release/4.3a0
Richard Roberts 2012-08-17 03:45:32 +00:00
parent 90b55a6ec1
commit fac426c5ac
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -61,6 +61,7 @@ struct DoglegOptimizerImpl {
*/
enum TrustRegionAdaptationMode {
SEARCH_EACH_ITERATION,
SEARCH_REDUCE_ONLY,
ONE_STEP_PER_ITERATION
};
@ -210,7 +211,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
const double dx_d_norm = result.dx_d.vector().norm();
const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
if(mode == ONE_STEP_PER_ITERATION)
if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)
stay = false; // If not searching, just return with the new Delta
else if(mode == SEARCH_EACH_ITERATION) {
if(newDelta == Delta || lastAction == DECREASED_DELTA)
@ -231,13 +232,17 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
} else if(0.25 > rho && rho >= 0.0) {
// M does not agree well with f, decrease Delta until it does
double newDelta;
if(Delta > 1e-5)
bool hitMinimumDelta;
if(Delta > 1e-5) {
newDelta = 0.5 * Delta;
else
hitMinimumDelta = false;
} else {
newDelta = Delta;
if(mode == ONE_STEP_PER_ITERATION || lastAction == INCREASED_DELTA || newDelta == Delta)
hitMinimumDelta = true;
}
if(mode == ONE_STEP_PER_ITERATION || /* mode == SEARCH_EACH_ITERATION && */ lastAction == INCREASED_DELTA || hitMinimumDelta)
stay = false; // If not searching, just return with the new smaller delta
else if(mode == SEARCH_EACH_ITERATION) {
else if(mode == SEARCH_EACH_ITERATION || mode == SEARCH_REDUCE_ONLY) {
stay = true;
lastAction = DECREASED_DELTA;
} else {
@ -246,7 +251,9 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
Delta = newDelta; // Update Delta from new Delta
} else {
// f actually increased, so keep decreasing Delta until f does not decrease
// f actually increased, so keep decreasing Delta until f does not decrease.
// NOTE: NaN and Inf solutions also will fall into this case, so that we
// decrease Delta if the solution becomes undetermined.
assert(0.0 > rho);
if(Delta > 1e-5) {
Delta *= 0.5;
@ -254,6 +261,8 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
lastAction = DECREASED_DELTA;
} else {
if(verbose) std::cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << std::endl;
result.dx_d.setZero(); // Set delta to zero - don't allow error to increase
result.f_error = f_error;
stay = false;
}
}