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

Use find

release/4.3a0
Fan Jiang 2020-06-02 15:38:04 -04:00
parent f73429133a
commit 50ffeb7dcd
2 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/linear/HessianFactor.cpp
View File

@ -313,8 +313,9 @@ VectorValues HessianFactor::hessianDiagonal() const {
/* ************************************************************************* */ /* ************************************************************************* */
void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { void HessianFactor::hessianDiagonalAdd(VectorValues &d) const {
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
if(d.exists(keys_[j])) { auto item = d.find(keys_[j]);
d.at(keys_[j]) += info_.diagonal(j); if(item != d.end()) {
item->second += info_.diagonal(j);
} else { } else {
d.emplace(keys_[j], info_.diagonal(j)); d.emplace(keys_[j], info_.diagonal(j));
} }

5
gtsam/linear/JacobianFactor.cpp
View File

@ -560,8 +560,9 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
model_->whitenInPlace(column_k); model_->whitenInPlace(column_k);
dj(k) = dot(column_k, column_k); dj(k) = dot(column_k, column_k);
} }
if(d.exists(j)) { auto item = d.find(j);
d.at(j) += dj; if(item != d.end()) {
item->second += dj;
} else { } else {
d.emplace(j, dj); d.emplace(j, dj);
} }