54 double dsqrReci = 1. / dsqr;
55 _kernel->robustify(dsqrReci * error, rho);
73 double sqrte = sqrt(e);
74 rho[0] = 2*sqrte*_delta - dsqr;
75 rho[1] = _delta / sqrte;
76 rho[2] = - 0.5 * rho[1] / e;
83 double dsqrReci = 1. / dsqr;
84 double aux1 = dsqrReci * e2 + 1.0;
85 double aux2 = sqrt(aux1);
86 rho[0] = 2 * dsqr * (aux2 - 1);
88 rho[2] = -0.5 * dsqrReci * rho[1] / aux1;
94 double dsqrReci = 1. / dsqr;
95 double aux = dsqrReci * e2 + 1.0;
96 rho[0] = dsqr * log(aux);
98 rho[2] = -dsqrReci * std::pow(rho[1], 2);
106 rho[2] = -2. * rho[1] * aux;
112 const double aux = e2 / dsqr;
113 const double aux2 = exp (-aux);
114 rho[0] = dsqr * (1. - aux2);
116 rho[2] = -aux2 / dsqr;
121 const double sqrte = sqrt(e2);
122 const double aux = sqrte /
_delta;
124 rho[1] = 1. / (1. + aux);
125 rho[2] = - 0.5 / (sqrte * (1. + aux));
130 const double e = sqrt(e2);
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;
138 rho[0] = delta2 / 3.;
162 const double& phi =
_delta;
163 double scale = (2.0*phi)/(phi+e2);
167 rho[0] = scale*e2*scale;
168 rho[1] = (scale*scale);
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
virtual void robustify(double e2, Vector3D &rho) const
RobustKernelScaleDelta(const RobustKernelPtr &kernel, double delta=1.)
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
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
std::shared_ptr< RobustKernel > RobustKernelPtr
void robustify(double error, Vector3D &rho) const
void setKernel(const RobustKernelPtr &ptr)
use another kernel for the underlying operation
virtual void robustify(double e2, Vector3D &rho) const