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

Timing statements

release/4.3a0
Richard Roberts 2012-03-19 16:55:52 +00:00
parent a558ad042e
commit b1d4552781
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -191,7 +191,6 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(verbose) cout << "rho = " << rho << endl;
if(rho >= 0.75) {
tic(7, "Rho >= 0.75");
// M agrees very well with f, so try to increase lambda
const double dx_d_norm = result.dx_d.vector().norm();
const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
@ -209,14 +208,12 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
assert(false); }
Delta = newDelta; // Update Delta from new Delta
toc(7, "Rho >= 0.75");
} else if(0.75 > rho && rho >= 0.25) {
// M agrees so-so with f, keep the same Delta
stay = false;
} else if(0.25 > rho && rho >= 0.0) {
tic(8, "0.25 > Rho >= 0.75");
// M does not agree well with f, decrease Delta until it does
double newDelta;
if(Delta > 1e-5)
@ -232,11 +229,8 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
assert(false); }
Delta = newDelta; // Update Delta from new Delta
toc(8, "0.25 > Rho >= 0.75");
}
else {
tic(9, "Rho < 0");
} else {
// f actually increased, so keep decreasing Delta until f does not decrease
assert(0.0 > rho);
if(Delta > 1e-5) {
@ -247,7 +241,6 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(verbose) cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << endl;
stay = false;
}
toc(9, "Rho < 0");
}
}

9
gtsam/nonlinear/ISAM2.cpp
View File

@ -591,9 +591,16 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
Values ISAM2::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues>
tic(1, "Copy Values");
Values ret(theta_);
toc(1, "Copy Values");
tic(2, "getDelta");
const Permuted<VectorValues>& delta(getDelta());
toc(2, "getDelta");
tic(3, "Expmap");
vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, getDelta(), ordering_, mask);
Impl::ExpmapMasked(ret, delta, ordering_, mask);
toc(3, "Expmap");
return ret;
}