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

Use faster version of in-place add

release/4.3a0
Fan Jiang 2020-06-02 16:17:28 -04:00
parent 50ffeb7dcd
commit 15fda68c1a
1 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/linear/JacobianFactor.cpp
View File

@ -552,6 +552,18 @@ VectorValues JacobianFactor::hessianDiagonal() const {
void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos];
auto item = d.find(j);
if(item != d.end()) {
size_t nj = Ab_(pos).cols();
Vector& dj = item->second;
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);
}
} else {
size_t nj = Ab_(pos).cols();
Vector dj(nj);
for (size_t k = 0; k < nj; ++k) {
@ -560,10 +572,7 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
model_->whitenInPlace(column_k);
dj(k) = dot(column_k, column_k);
}
auto item = d.find(j);
if(item != d.end()) {
item->second += dj;
} else {
d.emplace(j, dj);
}
}