robust.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <cmath>
00031 
00032 #include "util.h"
00033 
00034 namespace isam {
00035 
00036 /*
00037  * robust error functions following Hartley&Zisserman book (2nd edition, pages 616-622)
00038  * summary:
00039  * - squared error is not robust
00040  * - Blake-Zisserman, corrupted Gaussian and Cauchy functions are
00041  *   non-convex, and therefore require very good initialization
00042  * - L1 yields a stable minimum, the median, but is non-differentiable at the origin
00043  * - Huber cost function is convex, combining squared near the origin, L1 further out
00044  * - pseudo-Huber is a good alternative to Huber, with continuous derivatives of all orders
00045  */
00046 
00051 inline double cost_squared(double d) {
00052   return d*d;
00053 }
00054 
00060 inline double cost_blake_zisserman(double d, double e) {
00061   return -log(exp(-d*d) + e);
00062 }
00063 
00070 inline double cost_corrupted_gaussian(double d, double w, double a) {
00071   double d2 = d*d;
00072   double w2 = w*w;
00073   return -log(a*exp(-d2) + (1.-a)*exp(-d2/w2)/w);
00074 }
00075 
00082 inline double cost_cauchy(double d, double b = 1.) {
00083   // the first term is a constant and could be omitted
00084   return log(M_PI/b) * log(1. + d*d/(b*b));
00085 }
00086 
00092 inline double cost_l1(double d, double b = 0.5) {
00093   return 2.*b*fabs(d);
00094 }
00095 
00101 inline double cost_huber(double d, double b) {
00102   double abs_d = fabs(d);
00103   if (abs_d < b) {
00104     return d*d;
00105   } else {
00106     return 2*b*abs_d - b*b;
00107   }
00108 }
00109 
00115 inline double cost_pseudo_huber(double d, double b) {
00116   double b2 = b*b;
00117   return 2*b2*(sqrt(1+d*d/b2) - 1);
00118 }
00119 
00120 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Mar 1 2015 11:30:50