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

edited

release/4.3a0
lcarlone 2022-03-22 20:40:08 -04:00
parent f10dad2e39
commit 7b0356cbea
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/nonlinear/GncOptimizer.h
View File

@ -447,11 +447,11 @@ class GTSAM_EXPORT GncOptimizer {
return weights;
}
case GncLossType::TLS: { // use eq (14) in GNC paper
double upperbound = (mu + 1) / mu * barcSq_.maxCoeff();
double lowerbound = mu / (mu + 1) * barcSq_.minCoeff();
for (size_t k : unknownWeights) {
if (nfg_[k]) {
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
double upperbound = (mu + 1) / mu * barcSq_[k];
double lowerbound = mu / (mu + 1) * barcSq_[k];
weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu;
if (u2_k >= upperbound || weights[k] < 0) {
weights[k] = 0;