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

Replace the identity matrix with diagonal of Hessian.

release/4.3a0
hchiu 2014-02-15 20:46:28 -05:00
parent e789553261
commit b0618b8894
1 changed files with 2 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -118,6 +118,8 @@ void LevenbergMarquardtOptimizer::iterate() {
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
A.diagonal()=diagHessian.at(key_value.key);
Vector b = Vector::Zero(dim); Vector b = Vector::Zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, model); dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);