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

Traced Ceres and took sqrt of diagonal term as they did.

release/4.3a0
hchiu 2014-02-16 15:50:26 -05:00
parent e88b214b40
commit f804fd9bd1
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -114,15 +114,21 @@ void LevenbergMarquardtOptimizer::iterate() {
dampedSystem.reserve(dampedSystem.size() + state_.values.size());
// for each of the variables, add a prior
VectorValues hessianDiagonal;
if (params_.diagonalDamping)
if (params_.diagonalDamping) {
hessianDiagonal = linear->hessianDiagonal();
}
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
size_t dim = key_value.value.dim();
Matrix A = Matrix::Identity(dim, dim);
//Replace the identity matrix with diagonal of Hessian
if (params_.diagonalDamping)
A.diagonal() = hessianDiagonal.at(key_value.key);
if (params_.diagonalDamping) {
A.diagonal() = hessianDiagonal.at(key_value.key)*state_.lambda;
for (int aa=0; aa<dim; aa++)
{
A(aa,aa)=sqrt(A(aa,aa));
}
}
Vector b = Vector::Zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);