robust.h

Go to the documentation of this file.

#pragma once

#include <cmath>

#include "util.h"

namespace isam {

/*
 * robust error functions following Hartley&Zisserman book (2nd edition, pages 616-622)
 * summary:
 * - squared error is not robust
 * - Blake-Zisserman, corrupted Gaussian and Cauchy functions are
 *   non-convex, and therefore require very good initialization
 * - L1 yields a stable minimum, the median, but is non-differentiable at the origin
 * - Huber cost function is convex, combining squared near the origin, L1 further out
 * - pseudo-Huber is a good alternative to Huber, with continuous derivatives of all orders
 */

inline double cost_squared(double d) {
  return d*d;
}

inline double cost_blake_zisserman(double d, double e) {
  return -log(exp(-d*d) + e);
}

inline double cost_corrupted_gaussian(double d, double w, double a) {
  double d2 = d*d;
  double w2 = w*w;
  return -log(a*exp(-d2) + (1.-a)*exp(-d2/w2)/w);
}

inline double cost_cauchy(double d, double b = 1.) {
  // the first term is a constant and could be omitted
  return log(M_PI/b) * log(1. + d*d/(b*b));
}

inline double cost_l1(double d, double b = 0.5) {
  return 2.*b*fabs(d);
}

inline double cost_huber(double d, double b) {
  double abs_d = fabs(d);
  if (abs_d < b) {
    return d*d;
  } else {
    return 2*b*abs_d - b*b;
  }
}

inline double cost_pseudo_huber(double d, double b) {
  double b2 = b*b;
  return 2*b2*(sqrt(1+d*d/b2) - 1);
}

}