g2o
robust_kernel_impl.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #include "robust_kernel_impl.h"
28 #include "robust_kernel_factory.h"
29 
30 #include <cmath>
31 
32 namespace g2o {
33 
35  RobustKernel(delta),
36  _kernel(kernel)
37 {
38 }
39 
41  RobustKernel(delta)
42 {
43 }
44 
46 {
47  _kernel = ptr;
48 }
49 
50 void RobustKernelScaleDelta::robustify(double error, Vector3D& rho) const
51 {
52  if (_kernel.get()) {
53  double dsqr = _delta * _delta;
54  double dsqrReci = 1. / dsqr;
55  _kernel->robustify(dsqrReci * error, rho);
56  rho[0] *= dsqr;
57  rho[2] *= dsqrReci;
58  } else { // no robustification
59  rho[0] = error;
60  rho[1] = 1.;
61  rho[2] = 0.;
62  }
63 }
64 
65 void RobustKernelHuber::robustify(double e, Vector3D& rho) const
66 {
67  double dsqr = _delta * _delta;
68  if (e <= dsqr) { // inlier
69  rho[0] = e;
70  rho[1] = 1.;
71  rho[2] = 0.;
72  } else { // outlier
73  double sqrte = sqrt(e); // absolut value of the error
74  rho[0] = 2*sqrte*_delta - dsqr; // rho(e) = 2 * delta * e^(1/2) - delta^2
75  rho[1] = _delta / sqrte; // rho'(e) = delta / sqrt(e)
76  rho[2] = - 0.5 * rho[1] / e; // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e
77  }
78 }
79 
80 void RobustKernelPseudoHuber::robustify(double e2, Vector3D& rho) const
81 {
82  double dsqr = _delta * _delta;
83  double dsqrReci = 1. / dsqr;
84  double aux1 = dsqrReci * e2 + 1.0;
85  double aux2 = sqrt(aux1);
86  rho[0] = 2 * dsqr * (aux2 - 1);
87  rho[1] = 1. / aux2;
88  rho[2] = -0.5 * dsqrReci * rho[1] / aux1;
89 }
90 
91 void RobustKernelCauchy::robustify(double e2, Vector3D& rho) const
92 {
93  double dsqr = _delta * _delta;
94  double dsqrReci = 1. / dsqr;
95  double aux = dsqrReci * e2 + 1.0;
96  rho[0] = dsqr * log(aux);
97  rho[1] = 1. / aux;
98  rho[2] = -dsqrReci * std::pow(rho[1], 2);
99 }
100 
102 {
103  const double aux = _delta / (_delta + e2);
104  rho[0] = e2 * aux;
105  rho[1] = aux * aux;
106  rho[2] = -2. * rho[1] * aux;
107 }
108 
109 void RobustKernelWelsch::robustify(double e2, Vector3D& rho) const
110 {
111  const double dsqr = _delta * _delta;
112  const double aux = e2 / dsqr;
113  const double aux2 = exp (-aux);
114  rho[0] = dsqr * (1. - aux2);
115  rho[1] = aux2;
116  rho[2] = -aux2 / dsqr;
117 }
118 
119 void RobustKernelFair::robustify(double e2, Vector3D& rho) const
120 {
121  const double sqrte = sqrt(e2);
122  const double aux = sqrte / _delta;
123  rho[0] = 2. * _delta * _delta * (aux - log(1. + aux));
124  rho[1] = 1. / (1. + aux);
125  rho[2] = - 0.5 / (sqrte * (1. + aux));
126 }
127 
128 void RobustKernelTukey::robustify(double e2, Vector3D& rho) const
129 {
130  const double e = sqrt(e2);
131  const double delta2 = _delta * _delta;
132  if (e <= _delta) {
133  const double aux = e2 / delta2;
134  rho[0] = delta2 * (1. - std::pow((1. - aux), 3)) / 3.;
135  rho[1] = std::pow((1. - aux), 2);
136  rho[2] = -2. * (1. - aux) / delta2;
137  } else {
138  rho[0] = delta2 / 3.;
139  rho[1] = 0;
140  rho[2] = 0;
141  }
142 }
143 
144 
145 void RobustKernelSaturated::robustify(double e2, Vector3D& rho) const
146 {
147  double dsqr = _delta * _delta;
148  if (e2 <= dsqr) { // inlier
149  rho[0] = e2;
150  rho[1] = 1.;
151  rho[2] = 0.;
152  } else { // outlier
153  rho[0] = dsqr;
154  rho[1] = 0.;
155  rho[2] = 0.;
156  }
157 }
158 
159 //delta is used as $phi$
160 void RobustKernelDCS::robustify(double e2, Vector3D& rho) const
161 {
162  const double& phi = _delta;
163  double scale = (2.0*phi)/(phi+e2);
164  if(scale>=1.0)
165  scale = 1.0;
166 
167  rho[0] = scale*e2*scale;
168  rho[1] = (scale*scale);
169  rho[2] = 0;
170 }
171 
172 // register the kernel to their factory
182 } // end namespace g2o
virtual void robustify(double e2, Vector3D &rho) const
virtual void robustify(double e2, Vector3D &rho) const
Dynamic covariance scaling - DCS.
Eigen::Matrix< double, 3, 1, Eigen::ColMajor > Vector3D
Definition: eigen_types.h:46
virtual void robustify(double e2, Vector3D &rho) const
RobustKernelScaleDelta(const RobustKernelPtr &kernel, double delta=1.)
Huber Cost Function.
virtual void robustify(double e2, Vector3D &rho) const
Geman-McClure cost function.
#define G2O_REGISTER_ROBUST_KERNEL(name, classname)
virtual void robustify(double e2, Vector3D &rho) const
virtual void robustify(double e2, Vector3D &rho) const
Welsch cost function.
Pseudo Huber Cost Function.
virtual void robustify(double e2, Vector3D &rho) const
virtual void robustify(double e2, Vector3D &rho) const
base for all robust cost functions
Definition: robust_kernel.h:48
std::shared_ptr< RobustKernel > RobustKernelPtr
Definition: robust_kernel.h:74
void robustify(double error, Vector3D &rho) const
Saturated cost function.
Tukey Cost Function.
double delta() const
Definition: robust_kernel.h:69
void setKernel(const RobustKernelPtr &ptr)
use another kernel for the underlying operation
Cauchy cost function.
Fair cost function.
virtual void robustify(double e2, Vector3D &rho) const