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

9
gtsam/nonlinear/GncOptimizer.h
View File

@ -340,7 +340,7 @@ public:
// update weights of known inlier/outlier measurements
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) {
if (nfg_[k]) {
double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
@ -349,12 +349,14 @@ public:
}
}
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 lowerbound = mu / (mu + 1) * params_.barcSq;
for (size_t k : unknownWeights) {
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
if (u2_k >= upperbound) {
weights[k] = 0;
} else if (u2_k <= lowerbound) {
@ -365,6 +367,7 @@ public:
}
}
return weights;
}
default:
throw std::runtime_error(
"GncOptimizer::calculateWeights: called with unknown loss type.");