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

Speed Optimization: Move sqrt computation to hessianDiagonal storation as Luca suggested. Got same values in unit tests (TestNonlinearOptimizer).

release/4.3a0
hchiu 2014-02-28 18:09:10 -05:00
parent e127f07336
commit 037ed7b931
1 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -23,14 +23,18 @@
#include <gtsam/linear/Errors.h>
#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/map.hpp>
#include <string>
#include <cmath>
#include <fstream>
using namespace std;
namespace gtsam {
using boost::adaptors::map_values;
/* ************************************************************************* */
LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(const std::string &src) const {
std::string s = src; boost::algorithm::to_upper(s);
@ -123,6 +127,15 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
// Only retrieve diagonal vector when reuse_diagonal = false
if (params_.diagonalDamping && params_.reuse_diagonal_ == false) {
state_.hessianDiagonal = linear.hessianDiagonal();
BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values)
{
for( size_t aa = 0 ; aa < v.size() ; aa++)
{
v(aa) = std::min(std::max(v(aa), min_diagonal_),
max_diagonal_);
v(aa) = sqrt(v(aa));
}
}
}
// for each of the variables, add a prior
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
@ -131,12 +144,6 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
//Replace the identity matrix with diagonal of Hessian
if (params_.diagonalDamping) {
A.diagonal() = state_.hessianDiagonal.at(key_value.key);
for (size_t aa = 0; aa < dim; aa++) {
if (params_.reuse_diagonal_ == false)
A(aa, aa) = std::min(std::max(A(aa, aa), min_diagonal_),
max_diagonal_);
A(aa, aa) = sqrt(A(aa, aa));
}
}
Vector b = Vector::Zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);