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 { enum TrustRegionAdaptationMode {
SEARCH_EACH_ITERATION, SEARCH_EACH_ITERATION,
SEARCH_REDUCE_ONLY,
ONE_STEP_PER_ITERATION 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 dx_d_norm = result.dx_d.vector().norm();
const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta 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 stay = false; // If not searching, just return with the new Delta
else if(mode == SEARCH_EACH_ITERATION) { else if(mode == SEARCH_EACH_ITERATION) {
if(newDelta == Delta || lastAction == DECREASED_DELTA) if(newDelta == Delta || lastAction == DECREASED_DELTA)
@ -231,13 +232,17 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
} else if(0.25 > rho && rho >= 0.0) { } else if(0.25 > rho && rho >= 0.0) {
// M does not agree well with f, decrease Delta until it does // M does not agree well with f, decrease Delta until it does
double newDelta; double newDelta;
if(Delta > 1e-5) bool hitMinimumDelta;
if(Delta > 1e-5) {
newDelta = 0.5 * Delta; newDelta = 0.5 * Delta;
else hitMinimumDelta = false;
} else {
newDelta = Delta; 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 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; stay = true;
lastAction = DECREASED_DELTA; lastAction = DECREASED_DELTA;
} else { } else {
@ -246,7 +251,9 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
Delta = newDelta; // Update Delta from new Delta Delta = newDelta; // Update Delta from new Delta
} else { } 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); assert(0.0 > rho);
if(Delta > 1e-5) { if(Delta > 1e-5) {
Delta *= 0.5; Delta *= 0.5;
@ -254,6 +261,8 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
lastAction = DECREASED_DELTA; lastAction = DECREASED_DELTA;
} else { } else {
if(verbose) std::cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << std::endl; 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; stay = false;
} }
} }