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

fix scoping

release/4.3a0
jingnanshi 2020-12-07 15:08:50 -05:00
parent d85d9c6194
commit 58e49fc0fc
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/nonlinear/GncOptimizer.h
View File

@ -340,7 +340,7 @@ public:
// update weights of known inlier/outlier measurements // update weights of known inlier/outlier measurements
switch (params_.lossType) { switch (params_.lossType) {
case GncParameters::GM: // use eq (12) in GNC paper case GncParameters::GM: { // use eq (12) in GNC paper
for (size_t k : unknownWeights) { for (size_t k : unknownWeights) {
if (nfg_[k]) { if (nfg_[k]) {
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
@ -349,22 +349,25 @@ public:
} }
} }
return weights; return weights;
case GncParameters::TLS: // use eq (14) in GNC paper }
case GncParameters::TLS: { // use eq (14) in GNC paper
double upperbound = (mu + 1) / mu * params_.barcSq; double upperbound = (mu + 1) / mu * params_.barcSq;
double lowerbound = mu / (mu +1 ) * params_.barcSq; double lowerbound = mu / (mu + 1) * params_.barcSq;
for (size_t k : unknownWeights) { for (size_t k : unknownWeights) {
if (nfg_[k]) { if (nfg_[k]) {
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual double u2_k = nfg_[k]->error(
if (u2_k >= upperbound ) { currentEstimate); // squared (and whitened) residual
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 { } else {
weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k ) - mu; weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k) - mu;
} }
} }
} }
return weights; return weights;
}
default: default:
throw std::runtime_error( throw std::runtime_error(
"GncOptimizer::calculateWeights: called with unknown loss type."); "GncOptimizer::calculateWeights: called with unknown loss type.");