Go to the documentation of this file.
28 namespace noiseModel {
34 namespace mEstimator {
37 const size_t n =
error.rows();
39 for (
size_t i = 0;
i <
n; ++
i)
45 return weight(
error).cwiseSqrt();
52 if (reweight_ == Block) {
53 const double w = sqrtWeight(
error.norm());
56 error.array() *= weight(
error).cwiseSqrt().array();
62 if ( reweight_ == Block ) {
63 const double w = sqrtWeight(
error.norm());
80 if ( reweight_ == Block ) {
81 const double w = sqrtWeight(
error.norm());
94 if ( reweight_ == Block ) {
95 const double w = sqrtWeight(
error.norm());
110 if ( reweight_ == Block ) {
111 const double w = sqrtWeight(
error.norm());
131 { cout <<
s <<
"null ()" << endl; }
142 throw runtime_error(
"mEstimator Fair takes only positive double in constructor.");
152 const double normalizedError = absError /
c_;
153 const double c_2 =
c_ *
c_;
154 return c_2 * (normalizedError -
std::log1p(normalizedError));
158 { cout <<
s <<
"fair (" <<
c_ <<
")" << endl; }
162 if (
p ==
nullptr)
return false;
175 throw runtime_error(
"mEstimator Huber takes only positive double in constructor.");
181 return (absError <=
k_) ? (1.0) : (
k_ / absError);
186 if (absError <=
k_) {
189 return k_ * (absError - (
k_/2));
194 cout <<
s <<
"huber (" <<
k_ <<
")" << endl;
199 if (
p ==
nullptr)
return false;
213 throw runtime_error(
"mEstimator Cauchy takes only positive double in constructor.");
227 cout <<
s <<
"cauchy (" <<
k_ <<
")" << endl;
232 if (
p ==
nullptr)
return false;
246 throw runtime_error(
"mEstimator Tukey takes only positive double in constructor.");
253 return one_minus_xc2 * one_minus_xc2;
260 if (absError <=
c_) {
262 const double t = one_minus_xc2*one_minus_xc2*one_minus_xc2;
270 std::cout <<
s <<
": Tukey (" <<
c_ <<
")" << std::endl;
275 if (
p ==
nullptr)
return false;
300 std::cout <<
s <<
": Welsch (" <<
c_ <<
")" << std::endl;
305 if (
p ==
nullptr)
return false;
317 :
Base(reweight), c_(
c) {
322 const double c4 =
c2*
c2;
324 return c4/(c2error*c2error);
330 return 0.5 * (
c2 * error2) / (
c2 + error2);
334 std::cout <<
s <<
": Geman-McClure (" <<
c_ <<
")" << std::endl;
339 if (
p ==
nullptr)
return false;
351 :
Base(reweight), c_(
c) {
358 const double w = 2.0*
c_/(
c_ + e2);
369 const double e4 = e2*e2;
372 return (
c2*e2 +
c_*e4) / ((e2 +
c_)*(e2 +
c_));
376 std::cout <<
s <<
": DCS (" <<
c_ <<
")" << std::endl;
381 if (
p ==
nullptr)
return false;
394 :
Base(reweight), k_(k) {
396 throw runtime_error(
"mEstimator L2WithDeadZone takes only positive double in constructor.");
411 return (abs_error <
k_) ? 0.0 : 0.5*(
k_-abs_error)*(
k_-abs_error);
415 std::cout <<
s <<
": L2WithDeadZone (" <<
k_ <<
")" << std::endl;
420 if (
p ==
nullptr)
return false;
435 throw runtime_error(
"mEstimator AsymmetricTukey takes only positive double in constructor.");
445 return one_minus_xc2 * one_minus_xc2;
456 const double t = one_minus_xc2 * one_minus_xc2 * one_minus_xc2;
463 std::cout <<
s <<
": AsymmetricTukey (" <<
c_ <<
")" << std::endl;
468 if (
p ==
nullptr)
return false;
483 throw runtime_error(
"mEstimator AsymmetricCauchy takes only positive double in constructor.");
507 std::cout <<
s <<
": AsymmetricCauchy (" <<
k_ <<
")" << std::endl;
512 if (
p ==
nullptr)
return false;
525 Custom::Custom(std::function<
double(
double)> weight, std::function<
double(
double)> loss,
const ReweightScheme reweight,
538 std::cout <<
s <<
": Custom (" <<
name_ <<
")" << std::endl;
545 return name_ ==
p->name_ &&
weight_.target<double(
double)>() ==
p->weight_.target<
double(
double)>() &&
546 loss_.target<
double(
double)>() ==
p->loss_.target<
double(
double)>() &&
reweight_ ==
p->reweight_;
bool equals(const Base &expected, double tol=1e-8) const override
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Annotation for function names.
double weight(double distance) const override
bool equals(const Base &expected, double tol=1e-8) const override
AsymmetricCauchy(double k=0.1, const ReweightScheme reweight=Block)
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
std::shared_ptr< Fair > shared_ptr
void print(const std::string &s) const override
Welsch(double c=2.9846, const ReweightScheme reweight=Block)
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
void print(const std::string &s) const override
L2WithDeadZone(double k=1.0, const ReweightScheme reweight=Block)
AsymmetricTukey(double c=4.6851, const ReweightScheme reweight=Block)
std::function< double(double)> loss_
std::shared_ptr< GemanMcClure > shared_ptr
bool equals(const Base &expected, double tol=1e-8) const override
void print(const std::string &s) const override
Fair(double c=1.3998, const ReweightScheme reweight=Block)
void print(const std::string &s) const override
void print(const std::string &s) const override
bool equals(const Base &expected, double tol=1e-8) const override
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
bool equals(const Base &expected, double tol=1e-8) const override
void print(const std::string &s) const override
double loss(double distance) const override
double weight(double distance) const override
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
bool equals(const Base &expected, double tol=1e-8) const override
void print(const std::string &s) const override
std::shared_ptr< L2WithDeadZone > shared_ptr
double loss(double distance) const override
double loss(double distance) const override
ReweightScheme reweight_
Strategy for reweighting.
std::shared_ptr< AsymmetricTukey > shared_ptr
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
std::shared_ptr< Welsch > shared_ptr
void print(const std::string &s) const override
double weight(double distance) const override
std::shared_ptr< Base > shared_ptr
double loss(double distance) const override
std::function< double(double)> weight_
double loss(double distance) const override
double weight(double distance) const override
void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask)
std::shared_ptr< Huber > shared_ptr
bool equals(const Base &expected, double tol=1e-8) const override
std::shared_ptr< Null > shared_ptr
bool equals(const Base &expected, double tol=1e-8) const override
double loss(double distance) const override
DCS(double c=1.0, const ReweightScheme reweight=Block)
void print(const std::string &s) const override
bool equals(const Base &expected, double tol=1e-8) const override
bool equals(const Base &expected, double tol=1e-8) const override
Tukey(double c=4.6851, const ReweightScheme reweight=Block)
double loss(double distance) const override
std::shared_ptr< DCS > shared_ptr
std::shared_ptr< Tukey > shared_ptr
std::shared_ptr< Custom > shared_ptr
bool equals(const Base &expected, double tol=1e-8) const override
double weight(double distance) const override
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
double weight(double distance) const override
static shared_ptr Create(std::function< double(double)> weight, std::function< double(double)> loss, const ReweightScheme reweight=Block, const std::string &name="Custom")
GemanMcClure(double c=1.0, const ReweightScheme reweight=Block)
double weight(double distance) const override
detail::enable_if_t<!detail::move_never< T >::value, T > move(object &&obj)
double loss(double distance) const override
void print(const std::string &s) const override
std::shared_ptr< AsymmetricCauchy > shared_ptr
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Cauchy(double k=0.1, const ReweightScheme reweight=Block)
Double_ distance(const OrientedPlane3_ &p)
void print(const std::string &s) const override
std::shared_ptr< Cauchy > shared_ptr
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
double weight(double distance) const override
Huber(double k=1.345, const ReweightScheme reweight=Block)
double loss(double distance) const override
static shared_ptr Create(double c, const ReweightScheme reweight=Block)
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
double loss(double distance) const override
double weight(double distance) const override
void reweight(Vector &error) const
double loss(double distance) const override
double weight(double distance) const override
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
double weight(double distance) const override
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:44