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

ruled out corner case where weights are outside [0,1] in TLS

release/4.3a0
lcarlone 2022-01-05 23:01:57 -05:00
parent 0683dfaa67
commit c59fbc825f
1 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/GncOptimizer.h
View File

@ -439,13 +439,11 @@ class GTSAM_EXPORT GncOptimizer {
for (size_t k : unknownWeights) {
if (nfg_[k]) {
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
if (u2_k >= upperbound) {
weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu;
if (u2_k >= upperbound || weights[k] < 0) {
weights[k] = 0;
} else if (u2_k <= lowerbound) {
} else if (u2_k <= lowerbound || weights[k] > 1) {
weights[k] = 1;
} else {
weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k)
- mu;
}
}
}