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

made function name less ambiguous, added more comments on inlierThreshold

release/4.3a0
lcarlone 2020-12-21 22:32:34 -05:00
parent 0e09f019ef
commit cd82a56214
2 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/nonlinear/GncOptimizer.h
View File

@ -83,8 +83,11 @@ public:
}
/** Set the maximum weighted residual error for an inlier. For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega,
* the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier.
* In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq.
* Assuming a isotropic measurement covariance sigma^2 * Identity, the cost becomes: 0.5 * 1/sigma^2 || r(x) ||^2 <= barcSq.
* Hence || r(x) ||^2 <= 2 * barcSq * sigma^2
* */
void setInlierThreshold(const double inth) {
void setInlierCostThreshold(const double inth) {
barcSq = inth;
}
/// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep

8
tests/testGncOptimizer.cpp
View File

@ -254,7 +254,7 @@ TEST(GncOptimizer, calculateWeightsGM) {
double barcSq = 5.0;
weights_expected[3] =
std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50
gncParams.setInlierThreshold(barcSq);
gncParams.setInlierCostThreshold(barcSq);
auto gnc2 =
GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
weights_actual = gnc2.calculateWeights(initial, mu);
@ -315,7 +315,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) {
GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(
GncParams<GaussNewtonParams>::RobustLossType::TLS);
gncParams.setInlierThreshold(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier
gncParams.setInlierCostThreshold(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial, gncParams);
double mu = 1e6;
Vector weights_actual = gnc.calculateWeights(initial, mu);
@ -331,7 +331,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) {
GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(
GncParams<GaussNewtonParams>::RobustLossType::TLS);
gncParams.setInlierThreshold(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier
gncParams.setInlierCostThreshold(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial, gncParams);
double mu = 1e6; // very large mu recovers original TLS cost
Vector weights_actual = gnc.calculateWeights(initial, mu);
@ -347,7 +347,7 @@ TEST(GncOptimizer, calculateWeightsTLS2) {
GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(
GncParams<GaussNewtonParams>::RobustLossType::TLS);
gncParams.setInlierThreshold(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier
gncParams.setInlierCostThreshold(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(nfg, initial, gncParams);
double mu = 1e6; // very large mu recovers original TLS cost
Vector weights_actual = gnc.calculateWeights(initial, mu);