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

Cache noise models in LM damping instead of constructing from scratch for each variable. Time savings of 5.6%!

release/4.3a0
cbeall3 2015-10-20 16:25:41 -04:00
parent 04bcf26aa6
commit 1949fc2511
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -189,11 +189,19 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
} }
} else { } else {
// Straightforward damping: // Straightforward damping:
typedef map<size_t, SharedDiagonal> NoiseMap; // Cache noise models
NoiseMap noises;
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
size_t dim = key_value.value.dim(); size_t dim = key_value.value.dim();
Matrix A = Matrix::Identity(dim, dim); Matrix A = Matrix::Identity(dim, dim);
Vector b = Vector::Zero(dim); Vector b = Vector::Zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
// Check if noise model of appropriate size already exists, else create it and cache it!
NoiseMap::iterator it = noises.find(dim);
SharedDiagonal model = it == noises.end() ? noiseModel::Isotropic::Sigma(dim, sigma) : it->second;
if(it == noises.end()) {
noises[dim] = model;
}
damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model); damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
} }
} }