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

improved comments

release/4.3a0
lcarlone 2021-01-29 19:05:29 -05:00
parent a59a12245c
commit bd0a735ee8
1 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/nonlinear/GncOptimizer.h
View File

@ -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<double>::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.");