diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index dab2fb7be..ebdae765d 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -54,7 +54,7 @@ class GncOptimizer { Values state_; ///< Initial values to be used at each iteration by GNC. GncParameters params_; ///< GNC parameters. Vector weights_; ///< Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside). - Vector barcSq_; ///< Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance. Also note the code allows a threshold for each factor. + Vector barcSq_; ///< Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq_[i] (where i is the position of the factor in the factor graph. Note that factor.error() whitens by the covariance. public: /// Constructor. @@ -84,7 +84,7 @@ class GncOptimizer { /** Set the maximum weighted residual error for an inlier (same for all factors). 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. + * Assuming an 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 setInlierCostThresholds(const double inth) { @@ -94,8 +94,6 @@ class GncOptimizer { /** Set the maximum weighted residual error for an inlier (one for each factor). 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 setInlierCostThresholds(const Vector& inthVec) { barcSq_ = inthVec; @@ -105,7 +103,7 @@ class GncOptimizer { * alpha that the inlier residuals are smaller than that threshold * */ void setInlierCostThresholdsAtProbability(const double alpha) { - barcSq_ = Vector::Ones(nfg_.size()); + barcSq_ = Vector::Ones(nfg_.size()); // initialize 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 @@ -214,10 +212,12 @@ class GncOptimizer { double initializeMu() const { double mu_init = 0.0; - // set initial mu + // initialize mu to the value specified in Remark 5 in GNC paper. switch (params_.lossType) { case GncLossType::GM: - // surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper + /* surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper. + Since barcSq_ can be different for each factor, we compute the max of the quantity in remark 5 in GNC paper + */ for (size_t k = 0; k < nfg_.size(); k++) { if (nfg_[k]) { mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]); @@ -225,11 +225,11 @@ class GncOptimizer { } return mu_init; // initial mu case GncLossType::TLS: - /* initialize mu to the value specified in Remark 5 in GNC paper. - surrogate cost is convex for mu close to zero + /* surrogate cost is convex for mu close to zero. initialize as in remark 5 in GNC paper. degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop) according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual - however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop + however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop. + Since barcSq_ can be different for each factor, we look for the minimimum (positive) quantity in remark 5 in GNC paper */ mu_init = std::numeric_limits::infinity(); for (size_t k = 0; k < nfg_.size(); k++) { @@ -239,7 +239,9 @@ class GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } - return mu_init > 0 && !isinf(mu_init) ? mu_init : -1; + return mu_init > 0 && !isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, + // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold + // and there is no need to robustify (TLS = least squares) default: throw std::runtime_error( "GncOptimizer::initializeMu: called with unknown loss type.");