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

Further optimization

release/4.3a0
Fan Jiang 2020-06-02 23:52:38 -04:00
parent e8111a129d
commit f2a7864fb6
1 changed files with 14 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/linear/JacobianFactor.cpp
View File

@ -558,13 +558,20 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
Vector& dj = result.first->second;
for (size_t k = 0; k < nj; ++k) {
Vector column_k = Ab_(pos).col(k);
if (model_)
model_->whitenInPlace(column_k);
if(!result.second)
dj(k) += dot(column_k, column_k);
else
dj(k) = dot(column_k, column_k);
Eigen::Ref<const Vector> column_k = Ab_(pos).col(k);
if (model_) {
Vector column_k_copy = column_k;
model_->whitenInPlace(column_k_copy);
if(!result.second)
dj(k) += dot(column_k_copy, column_k_copy);
else
dj(k) = dot(column_k_copy, column_k_copy);
} else {
if (!result.second)
dj(k) += dot(column_k, column_k);
else
dj(k) = dot(column_k, column_k);
}
}
}
}