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

replaced all find&insert to emplace

release/4.3a0
Fan Jiang 2020-06-02 21:33:58 -04:00
parent 0675b82c1f
commit 151809c89c
3 changed files with 14 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/linear/HessianFactor.cpp
View File

@ -313,11 +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) {
auto item = d.find(keys_[j]); auto result = d.emplace(keys_[j], info_.diagonal(j));
if(item != d.end()) { if(!result.second) {
item->second += info_.diagonal(j); result.first->second += info_.diagonal(j);
} else {
d.emplace(keys_[j], info_.diagonal(j));
} }
} }
} }

19
gtsam/linear/JacobianFactor.cpp
View File

@ -552,29 +552,16 @@ VectorValues JacobianFactor::hessianDiagonal() const {
void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
for (size_t pos = 0; pos < size(); ++pos) { for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos]; Key j = keys_[pos];
auto item = d.find(j);
if(item != d.end()) {
size_t nj = Ab_(pos).cols(); size_t nj = Ab_(pos).cols();
Vector& dj = item->second; auto result = d.emplace(j, nj);
Vector& dj = result.first->second;
for (size_t k = 0; k < nj; ++k) { for (size_t k = 0; k < nj; ++k) {
Vector column_k = Ab_(pos).col(k); Vector column_k = Ab_(pos).col(k);
if (model_) if (model_)
model_->whitenInPlace(column_k); model_->whitenInPlace(column_k);
dj(k) += dot(column_k, column_k); dj(k) += dot(column_k, column_k);
} }
} else {
size_t nj = Ab_(pos).cols();
Vector dj(nj);
for (size_t k = 0; k < nj; ++k) {
Vector column_k = Ab_(pos).col(k);
if (model_)
model_->whitenInPlace(column_k);
dj(k) = dot(column_k, column_k);
}
d.emplace(j, dj);
}
} }
} }

8
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -172,11 +172,9 @@ public:
dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
} }
auto item = d.find(j); auto result = d.emplace(j, dj);
if(item != d.end()) { if(!result.second) {
item->second += dj; result.first->second += dj;
} else {
d.emplace(j, dj);
} }
} }
} }