implemented residual functions for common M-estimators and added corresponding tests
parent
3eb8e3d9bc
commit
8280a082bd
6
gtsam.h
6
gtsam.h
|
@ -1364,6 +1364,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
|
|||
void serializable() const;
|
||||
|
||||
double weight(double error) const;
|
||||
double residual(double error) const;
|
||||
};
|
||||
|
||||
virtual class Fair: gtsam::noiseModel::mEstimator::Base {
|
||||
|
@ -1384,6 +1385,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
|
|||
void serializable() const;
|
||||
|
||||
double weight(double error) const;
|
||||
double residual(double error) const;
|
||||
};
|
||||
|
||||
virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
|
||||
|
@ -1394,6 +1396,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
|
|||
void serializable() const;
|
||||
|
||||
double weight(double error) const;
|
||||
double residual(double error) const;
|
||||
};
|
||||
|
||||
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
|
||||
|
@ -1404,6 +1407,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
|
|||
void serializable() const;
|
||||
|
||||
double weight(double error) const;
|
||||
double residual(double error) const;
|
||||
};
|
||||
|
||||
virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
|
||||
|
@ -1414,6 +1418,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
|
|||
void serializable() const;
|
||||
|
||||
double weight(double error) const;
|
||||
double residual(double error) const;
|
||||
};
|
||||
|
||||
virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
|
||||
|
@ -1424,6 +1429,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
|
|||
void serializable() const;
|
||||
|
||||
double weight(double error) const;
|
||||
double residual(double error) const;
|
||||
};
|
||||
|
||||
//TODO DCS and L2WithDeadZone mEstimators
|
||||
|
|
|
@ -858,6 +858,12 @@ double GemanMcClure::weight(double error) const {
|
|||
return c4/(c2error*c2error);
|
||||
}
|
||||
|
||||
double GemanMcClure::residual(double error) const {
|
||||
const double c2 = c_*c_;
|
||||
const double error2 = error*error;
|
||||
return 0.5 * (c2 * error2) / (c2 + error2);
|
||||
}
|
||||
|
||||
void GemanMcClure::print(const std::string &s="") const {
|
||||
std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl;
|
||||
}
|
||||
|
|
|
@ -18,6 +18,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include<iostream>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
@ -727,6 +728,7 @@ namespace gtsam {
|
|||
Null(const ReweightScheme reweight = Block) : Base(reweight) {}
|
||||
virtual ~Null() {}
|
||||
virtual double weight(double /*error*/) const { return 1.0; }
|
||||
virtual double residual(double error) const { return error; }
|
||||
virtual void print(const std::string &s) const;
|
||||
virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
|
||||
static shared_ptr Create() ;
|
||||
|
@ -752,6 +754,12 @@ namespace gtsam {
|
|||
double weight(double error) const {
|
||||
return 1.0 / (1.0 + std::abs(error) / c_);
|
||||
}
|
||||
double residual(double error) const {
|
||||
double absError = std::abs(error);
|
||||
double normalizedError = absError / c_;
|
||||
double val = normalizedError - std::log(1 + normalizedError);
|
||||
return c_ * c_ * val;
|
||||
}
|
||||
void print(const std::string &s) const;
|
||||
bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;
|
||||
|
@ -779,6 +787,14 @@ namespace gtsam {
|
|||
double absError = std::abs(error);
|
||||
return (absError < k_) ? (1.0) : (k_ / absError);
|
||||
}
|
||||
double residual(double error) const {
|
||||
double absError = std::abs(error);
|
||||
if (absError <= k_) { // |x| <= k
|
||||
return error*error / 2;
|
||||
} else { // |x| > k
|
||||
return k_ * (absError - (k_/2));
|
||||
}
|
||||
}
|
||||
void print(const std::string &s) const;
|
||||
bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
|
||||
|
@ -809,6 +825,11 @@ namespace gtsam {
|
|||
double weight(double error) const {
|
||||
return ksquared_ / (ksquared_ + error*error);
|
||||
}
|
||||
double residual(double error) const {
|
||||
double normalizedError = error / k_;
|
||||
double val = std::log(1 + (normalizedError*normalizedError));
|
||||
return ksquared_ * val * 0.5;
|
||||
}
|
||||
void print(const std::string &s) const;
|
||||
bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
|
||||
|
@ -839,6 +860,16 @@ namespace gtsam {
|
|||
}
|
||||
return 0.0;
|
||||
}
|
||||
double residual(double error) const {
|
||||
double absError = std::abs(error);
|
||||
if (absError <= c_) {
|
||||
double t = csquared_ - (error*error);
|
||||
double val = t * t * t;
|
||||
return ((csquared_*csquared_*csquared_) - val) / (6.0 * csquared_ * csquared_);
|
||||
} else {
|
||||
return csquared_ / 6.0;
|
||||
}
|
||||
}
|
||||
void print(const std::string &s) const;
|
||||
bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
|
||||
|
@ -866,6 +897,10 @@ namespace gtsam {
|
|||
double xc2 = (error*error)/csquared_;
|
||||
return std::exp(-xc2);
|
||||
}
|
||||
double residual(double error) const {
|
||||
double xc2 = (error*error)/csquared_;
|
||||
return csquared_ * 0.5 * (1 - std::exp(-xc2) );
|
||||
}
|
||||
void print(const std::string &s) const;
|
||||
bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
|
||||
|
@ -897,6 +932,10 @@ namespace gtsam {
|
|||
double xc2 = (error*error)/csquared_;
|
||||
return std::exp(-xc2);
|
||||
}
|
||||
double residual(double error) const {
|
||||
double xc2 = (error*error)/csquared_;
|
||||
return csquared_ * 0.5 * (1 - std::exp(-xc2) );
|
||||
}
|
||||
void print(const std::string &s) const;
|
||||
bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
|
||||
|
@ -925,6 +964,7 @@ namespace gtsam {
|
|||
GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
|
||||
virtual ~GemanMcClure() {}
|
||||
virtual double weight(double error) const;
|
||||
virtual double residual(double error) const;
|
||||
virtual void print(const std::string &s) const;
|
||||
virtual bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
|
||||
|
|
|
@ -457,6 +457,22 @@ TEST(NoiseModel, WhitenInPlace)
|
|||
* penalties using iteratively re-weighted least squares.
|
||||
*/
|
||||
|
||||
TEST(NoiseModel, robustFunctionFair)
|
||||
{
|
||||
const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
|
||||
const mEstimator::Fair::shared_ptr fair = mEstimator::Fair::Create(k);
|
||||
DOUBLES_EQUAL(0.8333333333333333, fair->weight(error1), 1e-8);
|
||||
DOUBLES_EQUAL(0.3333333333333333, fair->weight(error2), 1e-8);
|
||||
// Test negative value to ensure we take absolute value of error.
|
||||
DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8);
|
||||
|
||||
DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8);
|
||||
DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8);
|
||||
DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionHuber)
|
||||
{
|
||||
const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
|
||||
|
@ -466,16 +482,74 @@ TEST(NoiseModel, robustFunctionHuber)
|
|||
// Test negative value to ensure we take absolute value of error.
|
||||
DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
|
||||
DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
|
||||
|
||||
DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8);
|
||||
DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8);
|
||||
DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionCauchy)
|
||||
{
|
||||
const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
|
||||
const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k);
|
||||
DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8);
|
||||
DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8);
|
||||
// Test negative value to ensure we take absolute value of error.
|
||||
DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8);
|
||||
|
||||
DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8);
|
||||
DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8);
|
||||
DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionGemanMcClure)
|
||||
{
|
||||
const double k = 1.0, error1 = 1.0, error2 = 10.0;
|
||||
const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
|
||||
const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k);
|
||||
const double weight1 = gmc->weight(error1),
|
||||
weight2 = gmc->weight(error2);
|
||||
DOUBLES_EQUAL(0.25 , weight1, 1e-8);
|
||||
DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8);
|
||||
DOUBLES_EQUAL(0.25 , gmc->weight(error1), 1e-8);
|
||||
DOUBLES_EQUAL(9.80296e-5, gmc->weight(error2), 1e-8);
|
||||
DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8);
|
||||
|
||||
DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8);
|
||||
DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8);
|
||||
DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionWelsch)
|
||||
{
|
||||
const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
|
||||
const mEstimator::Welsch::shared_ptr welsch = mEstimator::Welsch::Create(k);
|
||||
DOUBLES_EQUAL(0.960789439152323, welsch->weight(error1), 1e-8);
|
||||
DOUBLES_EQUAL(0.018315638888734, welsch->weight(error2), 1e-8);
|
||||
// Test negative value to ensure we take absolute value of error.
|
||||
DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8);
|
||||
|
||||
DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8);
|
||||
DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8);
|
||||
DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionTukey)
|
||||
{
|
||||
const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
|
||||
const mEstimator::Tukey::shared_ptr tukey = mEstimator::Tukey::Create(k);
|
||||
DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8);
|
||||
DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8);
|
||||
// Test negative value to ensure we take absolute value of error.
|
||||
DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8);
|
||||
|
||||
DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8);
|
||||
DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8);
|
||||
DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8);
|
||||
DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionDCS)
|
||||
|
|
Loading…
Reference in New Issue