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

avoided warning

release/4.3a0
Luca 2014-03-12 12:49:45 -04:00
parent b0cca0e4f0
commit b42a234c66
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -126,7 +126,7 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
if (params_.diagonalDamping && params_.reuse_diagonal_ == false) { if (params_.diagonalDamping && params_.reuse_diagonal_ == false) {
state_.hessianDiagonal = linear.hessianDiagonal(); state_.hessianDiagonal = linear.hessianDiagonal();
BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) {
for (size_t aa = 0; aa < v.size(); aa++) { for (int aa = 0; aa < v.size(); aa++) {
v(aa) = std::min(std::max(v(aa), min_diagonal_), max_diagonal_); v(aa) = std::min(std::max(v(aa), min_diagonal_), max_diagonal_);
v(aa) = sqrt(v(aa)); v(aa) = sqrt(v(aa));
} }
@ -141,13 +141,14 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
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);
//Replace the identity matrix with diagonal of Hessian //Replace the identity matrix with diagonal of Hessian
if (params_.diagonalDamping) if (params_.diagonalDamping){
try { try {
A.diagonal() = state_.hessianDiagonal.at(key_value.key); A.diagonal() = state_.hessianDiagonal.at(key_value.key);
} catch (std::exception e) { } catch (std::exception e) {
// Don't attempt any damping if no key found in diagonal // Don't attempt any damping if no key found in diagonal
continue; continue;
} }
}
Vector b = Vector::Zero(dim); Vector b = Vector::Zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model); damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);