Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add GNC threshold parameter #1901

Open
wants to merge 2 commits into
base: develop
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion examples/GNCExample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,8 @@ int main() {

// Set GNC-specific options
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
gncParams.setLossType(GncLossType::TLS);
gncParams.setLossType(GncLossType::TLS); // Truncated Least Squares or Geman-McClure
gncParams.setBarc(1.0); // Parameter $c$ in Truncated Least Squares or Geman-McClure

// Optimize the graph and print results
GncOptimizer<GncParams<LevenbergMarquardtParams>> optimizer(graph, initial, gncParams);
Expand Down
2 changes: 1 addition & 1 deletion gtsam/nonlinear/GncOptimizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ class GncOptimizer {
* alpha that the inlier residuals are smaller than that threshold
* */
void setInlierCostThresholdsAtProbability(const double alpha) {
barcSq_ = Vector::Ones(nfg_.size()); // initialize
barcSq_ = (params_.barc * params_.barc) * Vector::Ones(nfg_.size()); // initialize threshold to $bar{c}^2$
for (size_t k = 0; k < nfg_.size(); k++) {
if (nfg_[k]) {
barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the error definition in gtsam
Expand Down
7 changes: 7 additions & 0 deletions gtsam/nonlinear/GncParams.h
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ class GncParams {
double muStep = 1.4; ///< Multiplicative factor to reduce/increase the mu in gnc
double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
double barc = 1.0; ///< Default inlier threshold $\bar{c}$ = 1

Verbosity verbosity = SILENT; ///< Verbosity level

/// Use IndexVector for inliers and outliers since it is fast
Expand Down Expand Up @@ -108,6 +110,11 @@ class GncParams {
weightsTol = value;
}

/// Set the inlier threshold $\bar{c}$.
void setBarc(double value) {
barc = value;
}

/// Set the verbosity level.
void setVerbosityGNC(const Verbosity value) {
verbosity = value;
Expand Down