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

Using Eigen Identity and Zero functions in LM damp

release/4.3a0
Richard Roberts 2013-11-18 19:23:29 +00:00
parent 1f5f86f6e8
commit 45c022a791
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -102,8 +102,8 @@ void LevenbergMarquardtOptimizer::iterate() {
// for each of the variables, add a prior // for each of the variables, add a prior
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 = eye(dim); Matrix A = Matrix::Identity(dim, dim);
Vector b = 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);
} }